/* This program is for the 2014 RoboPlay Competition (Video) http://c-stem.ucdavis.edu
Video Title: MultiLinkbot Dance - The Lion Sleeps Tonite
School Name: Franklin High School
School District: Elk Grove Unified School District
Student Names: Rakeb Dawit, Brayden Lancashire, Nathan Urban
Teacher Advisor: Paul Akuna
 
File: S_Dancing.ch
made on 3~3~14
made by 
make four robots dance*/
#include <linkbot.h>
CLinkbotI robot1, robot2, robot3, robot4;
CLinkbotIGroup groupA, groupB, groupC;
/* connect to the linkbots*/
robot1.connect();
robot2.connect();
robot3.connect();
robot4.connect();
 
groupA.addRobot(robot1);
groupA.addRobot(robot2);
groupA.addRobot(robot3);
groupA.addRobot(robot4);
 
groupB.addRobot(robot1);
groupB.addRobot(robot2);
 
groupC.addRobot(robot3);
groupC.addRobot(robot4);
 
 
robot1.setColorRGB(255,0,0); //red
printf("Robot 1 is : RED \n");
robot2.setColorRGB(0,0,255); //blue
printf("Robot 2 is : BLUE \n");
robot3.setColorRGB(255,0,255); //purple
printf("Robot 3 is : PURPLE \n");
robot4.setColorRGB(255,255,0); //yellow
printf("Robot 4 is : YELLOW  \n");
 
groupA.moveNB(233.8,NaN,233.8);
 
robot1.delaySeconds(3);
 
groupB.moveNB(360,NaN,-360);
groupC.moveNB(-360,NaN,360);
groupC.moveNB(360,NaN,-360);
groupB.moveNB(-360,NaN,360);
 
robot1.delaySeconds(14);
 
robot1.setColorRGB(255,255,0); //yellow
printf("Robot 1 is : YELLOW \n");
robot2.setColorRGB(255,0,0); //red
printf("Robot 2 is : RED \n");
robot3.setColorRGB(0,0,255); //blue
printf("Robot 3 is : BLUE \n");
robot4.setColorRGB(255,0,255); //purple
printf("Robot 4 is : PURPLE \n");
 
robot1.delaySeconds(5);
 
robot1.setJointSpeeds(50,NaN,50);
robot2.setJointSpeeds(50,NaN,50);
robot3.setJointSpeeds(50,NaN,50);
robot4.setJointSpeeds(50,NaN,50);
 
groupB.moveNB(360,NaN,-360);
groupC.moveNB(360,NaN,-360);
groupC.moveNB(360,NaN,-360);
groupB.moveNB(360,NaN,-360);
 
robot1.delaySeconds(6);
 
robot1.setJointSpeeds(50,NaN,50);
robot2.setJointSpeeds(50,NaN,50);
robot3.setJointSpeeds(50,NaN,50);
robot4.setJointSpeeds(50,NaN,50);
 
groupB.moveNB(360,NaN,-360);
groupC.moveNB(360,NaN,-360);
groupC.moveNB(360,NaN,-360);
groupB.moveNB(360,NaN,-360);
 
robot1.delaySeconds(6);
 
robot1.setColorRGB(255,0,255); //purple
printf("Robot 1 is : PURPLE \n");
robot2.setColorRGB(255,255,0); //yellow
printf("Robot 2 is : YELLOW \n");
robot3.setColorRGB(255,0,0); //red
printf("Robot 3 is : RED \n");
robot4.setColorRGB(0,0,255); //blue
printf("Robot 4 is : BLUE \n");
 
robot1.delaySeconds(7);
 
robot1.setJointSpeeds(50,NaN,50);
robot2.setJointSpeeds(50,NaN,50);
robot3.setJointSpeeds(50,NaN,50);
robot4.setJointSpeeds(50,NaN,50);
 
groupB.moveNB(360,NaN,-360);
groupC.moveNB(360,NaN,-360);
groupC.moveNB(360,NaN,-360);
groupB.moveNB(360,NaN,-360);
 
robot1.delaySeconds(7.5);
 
robot1.setColorRGB(0,0,255); //blue
printf("Robot 1 is : BLUE \n");
robot2.setColorRGB(255,0,255); //purple
printf("Robot 2 is : PURPLE \n");
robot3.setColorRGB(255,255,0); //yellow
printf("Robot 3 is : YELLOW \n");
robot4.setColorRGB(255,0,0); //red
printf("Robot 4 is : RED \n");
 
robot1.delaySeconds(7);
 
robot1.setColorRGB(255,0,0); //red
printf("Robot 1 is : RED \n");
robot2.setColorRGB(0,0,255); //blue
printf("Robot 2 is : BLUE \n");
robot3.setColorRGB(255,0,255); //purple
printf("Robot 3 is : PURPLE \n");
robot4.setColorRGB(255,255,0); //yellow
printf("Robot 4 is : YELLOW \n");
 
robot1.delaySeconds(5);
 
groupA.moveNB(1000,NaN,1000);
 
robot1.delaySeconds(5);
 
robot1.setColorRGB(255,255,0); //yellow
printf("Robot 1 is : YELLOW \n");
robot2.setColorRGB(255,0,0); //red
printf("Robot 2 is : RED \n");
robot3.setColorRGB(0,0,255); //blue
printf("Robot 3 is : BLUE \n");
robot4.setColorRGB(255,0,255); //purple
printf("Robot 4 is : PURPLE \n");
 
robot1.delaySeconds(16);
 
robot1.setColorRGB(255,0,255); //purple
printf("Robot 1 is : PURPLE \n");
robot2.setColorRGB(255,255,0); //yellow
printf("Robot 2 is : YELLOW \n");
robot3.setColorRGB(255,0,0); //red
printf("Robot 3 is : RED \n");
robot4.setColorRGB(0,0,255); //blue
printf("Robot 4 is : BLUE \n");
 
robot1.delaySeconds(31);
 
robot1.setColorRGB(0,0,255); //blue
printf("Robot 1 is : BLUE \n");
robot2.setColorRGB(255,0,255); //purple
printf("Robot 2 is : PURPLE \n");
robot3.setColorRGB(255,255,0); //yellow
printf("Robot 3 is : YELLOW \n");
robot4.setColorRGB(255,0,0); //red
printf("Robot 4 is : RED \n");
 
robot1.delaySeconds(16);
 
robot1.setColorRGB(255,0,0); //red
printf("Robot 1 is : RED \n");
robot2.setColorRGB(0,0,255); //blue
printf("Robot 2 is : BLUE \n");
robot3.setColorRGB(255,0,255); //purple
printf("Robot 3 is : PURPLE \n");
robot4.setColorRGB(255,255,0); //yellow
printf("Robot 4 is : YELLOW \n");