/* File Name: tank.ch
This program is the for 2014 RoboPlay Competition (Video)
http://c-stem.ucdavis.edu
Video Title: Breaking News
School Name: Luther Burbank High School
School District: Sacramento City Unified School District
Student Names: Shue Thao, Jerry Pham, Manuel Cortes, Antonio Garcia
Teacher Advisors: Mr. Dagler and Mr. Williams
*/
 
#include <linkbot.h>
 
#define numOfSteps 1
#define angle 30
#define delayInSecs 0
 
CLinkbotL robot1, robot2, robot5;
 
CLinkbotI robot3, robot4;
 
robot1.connect();
robot2.connect();
robot3.connect();
robot4.connect();
robot5.connect();
 
double radius = 1.75;      
double trackwidth = 3.69;  
double speed = 25;  
 
double radius1 = 1.75;      
double trackwidth1 = 3.69;  
double speed1 = 25;  
 
double radius2 = 1.75;      
double trackwidth2 = 3.69;  
double speed2 = 25;  
 
robot1.setJointSpeeds(speed, NaN, speed);
robot2.setJointSpeeds(speed, NaN, speed);
robot3.setJointSpeeds(speed1, NaN, speed1);
robot4.setJointSpeeds(speed1, NaN, speed1);
 
int i=0;
 
for(i=0; i<numOfSteps; i++) {
    /*
     * Going up
     */
    robot1.moveNB(180, 0, NaN);
    robot2.moveNB(-180, 0, NaN);
    robot3.moveNB(-180, NaN, 180);
    robot4.moveNB(-180, NaN, 180);
    robot1.moveWait();
    robot2.moveWait();
    robot3.moveWait();
    robot4.moveWait();
 
    delay(delayInSecs);
 
    robot1.moveNB(0, angle, NaN);
    robot2.moveNB(0, angle, NaN);
    robot1.moveWait();
    robot2.moveWait();
 
    delay(delayInSecs);
 
    robot1.setJointSpeeds(speed2, NaN, speed2);
    robot2.setJointSpeeds(speed2, NaN, speed2);
    robot3.setJointSpeeds(speed2, NaN, speed2);
    robot4.setJointSpeeds(speed2, NaN, speed2);
 
    robot1.moveNB(280, 0, NaN);
    robot2.moveNB(-280, 0, NaN);
    robot3.moveNB(180, NaN, 280);
    robot4.moveNB(180, NaN, 280);
    robot3.moveWait();
    robot4.moveWait();
 
    delay(delayInSecs);
 
}
 
robot5.move(0, -90, NaN);
robot5.move(20, 0, NaN);