/* * * * * * * * * * * * * * * * * * * * * * 
 * multi-robot-demoB.c :
 * A demo of multiple robots with non-blocking commands in MyroC
 *
 * Description: 3 robots will play the Super Mario Bros. theme song while
 * driving in circles.   
 *
 * Authors: Anita DeWitt, Nicolas Knoebber
 * Date: September 13, 2014
 *
 * Initial setup: Wilkes, Perlis, and Curry should be side by side an 
 * inch and half apart. Make sure that their wheels match up in a line.
 *  Make sure that Perlis is in the center. Wilkes and Perlis should be
 *  facing the same direction with Curry facing in the opposite
 *  direction.

 *    
 * If using different robots, change the MAC addresses in rConnect()
 * These can be found using the command "hcitool scan"
 * Variable names need not change.
 * 
 * compile with the line:
 *  gcc -lbluetooth -l jpeg -lMyroC -o multi-robot-demoB multi-robot-demoB.c
 * 
 * * * * * * * * * * * * * * * * * * * * * */
  
  
#include "MyroC.h"
#include <stdio.h>
  
int curry,Perlis,wilkes;

//define durations of notes
double quarter = .25;
double eighth = .125;
double half = .5;
double whole = 1.0;
double dottedeighth = .375;
double boxcartrip = .167;

//define notes as frequencies
const int pitchA4  = 440;
const int pitchBf4 = 466;
const int pitchAs4 = 466; 
const int pitchB4  = 493;
const int pitchC5  = 523;
const int pitchDf5 = 554;
const int pitchCs5 = 554; 
const int pitchD5  = 587;
const int pitchEf5 = 622;
const int pitchDs5 = 622;
const int pitchE5  = 659;
const int pitchF5  = 698;
const int pitchGf5 = 739;
const int pitchFs5 = 739; 
const int pitchG5  = 783;
const int pitchAf5 = 830;
const int pitchGs5 = 830;
const int pitchA5  = 880;
const int pitchBf5 = 932;
const int pitchAs5 = 932;
const int pitchB5  = 987;
const int pitchG6 = 1567;
const int pitchG4 = 392;
const int pitchE4 = 329;
const int pitchBb4 = 466;
const int pitchGb5 = 739;
const int pitchEb5 = 622;
const int pitchAb5 = 830;
const int pitchC6 = 1046;
  
int main() 
{
  //assign mac addresses for each robot
  curry  = rConnect ("00:1E:19:01:0D:D1");
  Perlis = rConnect ("B4:D8:A9:00:09:58");
  wilkes= rConnect("00:1E:19:01:0E:13");
          
  //Connect to each robot and position them before movement
  rSetConnection (curry);
  rForward(1,1);
  rSetConnection(wilkes);
  rForward(1,2);
  rSetConnection(Perlis);

  //Curry begins circle movement
  rSetConnection (curry);
  rTurnLeft(1, 0.773);
  rMotors(.5,1.0);

  //Wilkes begins cricle
  rSetConnection(wilkes);
  rTurnLeft(.75, 1.22);
  rMotors(.5,1);

  //Perlis begins circle
  rSetConnection(Perlis);
  rTurnLeft(1.0,.773);
  rMotors(.2,1);
   
  //Curry sings the first part
  rSetConnection(curry);
  rBeep(eighth, pitchE5);
  rBeep(eighth, pitchE5);
  rBeep(eighth, 0);
  rBeep(eighth, pitchE5);
  rBeep(eighth, 0);
  rBeep(eighth, pitchC5);
  rBeep(quarter, pitchE5);
  rBeep(quarter, pitchG5);
  rBeep(quarter, 0);
  rBeep(quarter, pitchG4);
  rBeep(quarter, 0);
  
  //Wilkes sings the second part
  rSetConnection(wilkes);
  rBeep(quarter, 0);
  rBeep(dottedeighth, pitchC5);
  rBeep(eighth, pitchG4);
  rBeep(quarter, 0);
  rBeep(dottedeighth, pitchE4);
  rBeep(quarter, pitchA4);
  rBeep(quarter, pitchB4);
  rBeep(eighth, pitchBb4);
  rBeep(quarter, pitchA4);
  rBeep(boxcartrip, pitchG4);
  rBeep(boxcartrip, pitchE5);
  rBeep(boxcartrip, pitchG5);
  rBeep(quarter, pitchA5);
  rBeep(eighth, pitchF5);
  rBeep(eighth, pitchG5);
  rBeep(eighth, 0);
  rBeep(quarter, pitchE5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, pitchD5);
  rBeep(quarter, pitchB4);
  
  rBeep(quarter, 0);
  rBeep(dottedeighth, pitchC5);
  rBeep(eighth, pitchG4);
  rBeep(quarter, 0);
  rBeep(dottedeighth, pitchE4);
  rBeep(quarter, pitchA4);
  rBeep(quarter, pitchB4);
  rBeep(eighth, pitchBb4);
  rBeep(quarter, pitchA4);
  rBeep(boxcartrip, pitchG4);
  rBeep(boxcartrip, pitchE5);
  rBeep(boxcartrip, pitchG5);
  rBeep(quarter, pitchA5);
  rBeep(eighth, pitchF5);
  rBeep(eighth, pitchG5);
  rBeep(eighth, 0);
  rBeep(quarter, pitchE5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, pitchD5);
  rBeep(quarter, pitchB4);
  
  //Perlis sings the third part
  rSetConnection(Perlis);
  rBeep(quarter, 0);
  rBeep(eighth, pitchG5);
  rBeep(eighth, pitchGb5);
  rBeep(eighth, pitchF5);
  rBeep(quarter, pitchEb5);
  rBeep(eighth, pitchE5);
  
  rBeep(eighth, 0);
  rBeep(eighth, pitchAb5);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, 0);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, pitchD5);
  
  rBeep(eighth, 0);
  rBeep(eighth, pitchC6);
  rBeep(eighth, 0);
  rBeep(eighth, pitchC6);
  rBeep(half, pitchC6);
  
  rBeep(quarter, 0);
  rBeep(eighth, pitchG5);
  rBeep(eighth, pitchGb5);
  rBeep(eighth, pitchF5);
  rBeep(quarter, pitchEb5);
  rBeep(eighth, pitchE5);
  
  rBeep(eighth, 0);
  rBeep(eighth, pitchAb5);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, 0);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, pitchD5);
  
  rBeep(quarter, 0);
  rBeep(quarter, pitchEb5);
  rBeep(eighth, 0);
  rBeep(quarter, pitchD5);
  rBeep(eighth, 0);
  rBeep(.5,523);
  
  rBeep(quarter, 0);
  rBeep(eighth, pitchG5);
  rBeep(eighth, pitchGb5);
  rBeep(eighth, pitchF5);
  rBeep(quarter, pitchEb5);
  rBeep(eighth, pitchE5);
  
  rBeep(eighth, 0);
  rBeep(eighth, pitchAb5);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, 0);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, pitchD5);
  
  rBeep(eighth, 0);
  rBeep(eighth, pitchC6);
  rBeep(eighth, 0);
  rBeep(eighth, pitchC6);
  rBeep(half, pitchC6);
  
  rBeep(quarter, 0);
  rBeep(eighth, pitchG5);
  rBeep(eighth, pitchGb5);
  rBeep(eighth, pitchF5);
  rBeep(quarter, pitchEb5);
  rBeep(eighth, pitchE5);
  
  rBeep(eighth, 0);
  rBeep(eighth, pitchAb5);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, 0);
  rBeep(eighth, pitchA5);
  rBeep(eighth, pitchC5);
  rBeep(eighth, pitchD5);
  
  rBeep(quarter, 0);
  rBeep(quarter, pitchEb5);
  rBeep(eighth, 0);
  rBeep(quarter, pitchD5);
  rBeep(eighth, 0);
  rBeep(.5, 523);
  
  //Robots disconnect and stop
  rSetConnection (curry);
  rStop();
  rDisconnect();
  rSetConnection(wilkes);
  rStop();
  rDisconnect();
  rSetConnection(Perlis);
  rStop();
  rDisconnect();
  
  return 0;
}