/* File: BatmanLogo.ch */
/*
Using RoboSim:
Robot 1 will be automatically at position (0, 0)
Robot 2 will be automatically at position (6, 0)

*/
   
#include <linkbot.h>
CLinkbotI robot1, robot2;
double radius = 1.75;
double trackwidth = 3.69;
double speed = 7;
double x, y;
int i = 0;
int j = 0;
//Set the speeds of the robots
robot1.setSpeed(speed, radius);
robot2.setSpeed(speed, radius);

//Drive robot 1 to the initial position
robot1.traceOff();
robot1.drivexyTo(0, 14, radius, trackwidth);
robot1.traceOn();

//Draw the left side of the head.
robot1.drivexyTo(-2, 14, radius, trackwidth);
robot1.drivexyTo(-2.6, 16, radius, trackwidth);
robot1.drivexyTo(-4, 9, radius, trackwidth);
robot1.turnRight(80, radius, trackwidth);

//Drive robot 2 to the initial position
robot2.traceOff();
robot2.drivexyTo(0, 14, radius, trackwidth);
robot2.traceOn();

//Draw the right side of the head.
robot2.drivexyTo(2, 14, radius, trackwidth);
robot2.drivexyTo(2.6, 16, radius, trackwidth);
robot2.drivexyTo(4, 9, radius, trackwidth);
robot2.turnLeft(80, radius, trackwidth);

//Draw the first curve
while(i < 20){
robot1.driveDistanceNB(0.5, radius);
robot2.driveDistance(0.5, radius);
robot1.drivexyWait();
robot2.drivexyWait();
robot1.turnRight(5, radius, trackwidth);
robot2.turnLeft(5, radius, trackwidth);
i++;
}

// Draw the second curve
robot1.turnLeft(110, radius, trackwidth);
robot2.turnRight(110, radius, trackwidth);
while(j < 32){
    robot1.driveDistanceNB(1, radius);
    robot2.driveDistance(1, radius);
    robot1.drivexyWait();
    robot2.drivexyWait();
    robot1.turnLeft(5, radius, trackwidth);
    robot2.turnRight(5, radius, trackwidth);
    j++;
}

//Draw the third curve
robot1.turnLeft(100, radius, trackwidth);
robot2.turnRight(100, radius, trackwidth);
i = 0;
double angle = 4;
while(i < 25){
    robot1.driveDistanceNB(0.3, radius);
    robot2.driveDistance(0.3, radius);
    robot1.drivexyWait();
    robot2.drivexyWait();
    robot1.turnRight(angle, radius, trackwidth);
    robot2.turnLeft(angle, radius, trackwidth);
    i++;
    angle = angle + 0.3;
}

//Draw the last curve for robot 1
robot1.turnLeft(170, radius, trackwidth);
robot2.turnRight(170, radius, trackwidth);
i = 0;
angle = 4;
while(i < 23){
    robot1.driveDistance(0.4, radius);
    robot1.turnRight(angle, radius, trackwidth);
    i++;
    angle = angle + 0.3;
    if(angle >= 9){
        angle = 6;
    }
}
robot1.traceOff();
robot1.movexyToNB(-12, 24, radius, trackwidth); //Move robot 1 away
i = 0;
angle = 4;
//Draw last curve for robot 2.
while(i < 23){
    robot2.driveDistance(0.4, radius);
    robot2.turnLeft(angle, radius, trackwidth);
    i++;
    angle = angle + 0.3;
    if(angle >= 9){
        angle = 6;
    }
}