/* 2017 RoboPlay Video Competition
Division #:2
Video Title: [Robo Story] - Scene 14 a
Teacher Advisor: Matthew D Huffine
School Name: Academy for Academic Excellence
Student Names: [Karim Alkawass	, Jenive Arostegui, Janelle Bean, Austin Delahousie, Benjamin Fuller, Nicholas Galaviz, Sean Patrick Lauron, Tadhg   Nolan, Jessica Rogado, David Stanton, Noah Williams, Vivian Penatello, Matthew Martinez]
Code written by: [Jessica Rogado and David Stanton]
Purpose: [1 robot spells out robot]
*/
 
#include <linkbot.h> // access linkbot directory
#include <chplot.h> // access ch  plot
CLinkbotI robot; // declares linkbotI as robot
CPlot plot; // plot
robotRecordData_t timedata, distances; // record robot data
double timeInterval = 0.1; // save timeInterval = .1
int numDataPoints; // intialise number data points
double radius = 1.75; // radius = 1.75
double trackwidth = 3.69; // trackwidth = 3.69
double R1,R2; // double R1 and R2 in "Robot"
double o1,o2,o3,o4; // double o1, o2, o3, o4 in "Robot"
double b1,b2; // double b1 and b2 in "Robot"
double t1,t2; // double t1, t2 in "Robot"
double total1, total2; // double total1 and total 2 
double totally; // double totally 
double elaspedtimeR, elapsedtimeo1, elapsedtimeo2, elapsedtimeb, elapsedtimet; // elaasped time for each letter of "Robot"
 
robot.resetToZero();
robot.setLEDColor("black"); // set color to white -- not writing a letter
robot.setLEDColor("red"); // set color to red -- writing a letter
 
robot.recordDistanceBegin(timedata, distances, radius, timeInterval); // start to record distance for R
robot.systemTime(total1); // start to record system time for whole word -- "Robot"
robot.systemTime(R1); // start to record system time R
robot.driveDistance(25, radius); // drive forward 25 inches 
robot.turnRight(90, radius, trackwidth); // turn right 90 
robot.setJointSpeeds(105, NaN, 55); // set joint speed
robot.move(800, NaN, -405); // move wheel
robot.setJointSpeeds(70,NaN, 70); // set joint speed
robot.turnLeft(145, radius, trackwidth); // turn left 145 degrees
robot.driveDistance(18, radius); // drive forward 18 inches 
robot.turnLeft(46, radius, trackwidth); // turn left 46 degrees
robot.systemTime(R2); // end record system time R
elaspedtimeR = R2 - R1; // calcuate the seconds it takes for Linkbot to drive the letter R
printf("The motion for the robot took %.2lf seconds\n", elaspedtimeR); // print statement
 
robot.setLEDColor("black"); // no light
robot.driveDistance(8, radius); // space
 
robot.setLEDColor("red"); // set color to red
robot.systemTime(o1); // start to record system time for the first O in Robot
robot.setJointSpeeds(50, NaN, 100); // set joint speeds
robot.move(770, NaN, -1550); // move wheel
robot.systemTime(o2); // end record system time for the first O in Robot
elapsedtimeo1 = o2 - o1; // calcuate the seconds it takess for Linkbot to drive the letter O
printf("The motion for the robot took %.2lf seconds\n", elapsedtimeo1); // print statement
 
robot.setLEDColor("black"); // no light
robot.setJointSpeeds(70,NaN,70); // set speed
robot.driveDistance(10, radius); // space
 
robot.setLEDColor("red"); // set color to red
robot.systemTime(b1); // start to record system time for B in Robot
robot.turnLeft(90, radius, trackwidth); // turn left 90 degrees
robot.driveDistance(24, radius); // drive forward 24 inches
robot.driveDistance(-12, radius); // drive backward 12 inches
robot.turnLeft(90, radius, trackwidth); // turn left 90 degrees
robot.setJointSpeeds(55, NaN, 105); // set joint speeds
robot.move(-405, NaN, 800); // move wheel
robot.systemTime(b2); // ened system time for B in Robot
elapsedtimeb = b2 - b1; // calculate the seconds it takes for the Linkbot to drive the letter B
printf("The motion for the robot took %.2lf seconds\n", elapsedtimeb); // print statement
 
robot.setLEDColor("black"); // set color to white
robot.setJointSpeeds(70,NaN,70); // set joint speed
robot.driveDistance(13, radius); // space
 
robot.setLEDColor("red"); // set color to red
robot.systemTime(o3); // start to record system time for O in Robot
robot.setJointSpeeds(50, NaN, 100); // set joint speed
robot.move(770, NaN, -1550); // 
robot.systemTime(o4); // stop to record system time for o in robot
elapsedtimeo2 = o4 - o3; // calculate the secs it takes for the Linkbot to drive the letter O
printf("The motion for the robot took %.2lf seconds\n", elapsedtimeo2); // print statement
 
robot.setLEDColor("black"); // set color to white
robot.setJointSpeeds(70,NaN,70); // set joint speeds
robot.driveDistance(10, radius); // space
 
robot.setLEDColor("red"); // set color to red
robot.systemTime(t1); // start to record system tiem for t in Robot
robot.turnLeft(90, radius, trackwidth); // turn left 90
robot.driveDistance(24, radius); // drive foward 24 inches
robot.driveDistance(-8, radius); // drive back 8 inches
robot.turnRight(90, radius, trackwidth); // turn right 90 degrees
robot.driveDistance(3, radius); // drive 3 inches
robot.driveDistance(-6, radius); // drive backward 6 inches
robot.driveDistance(3, radius); // drive forward 3 inches
robot.turnLeft(90, radius, trackwidth);// turn left 90 degrees
robot.driveDistance(-18, radius);// drive 18 inches backward
robot.recordDistanceEnd(numDataPoints); // end distance record
robot.systemTime(t2); // end system time
elapsedtimet = t2 - t1; // calculate time 
printf("The motion for the robot took %.2lf seconds\n", elapsedtimet); // print statement
robot.setLEDColor("black"); // set color to white
 
 
robot.systemTime(total2); // end system time 
 
totally = total2 - total1; // calculate the total distance it takes for the linkbot to spell Robot
printf("The time it takes to spell out Robot is %.2lf seconds\n", totally); // print statement
 
plot.title("Position");  // plot position
plot.label(PLOT_AXIS_X, "X (time)"); // label x axis
plot.label(PLOT_AXIS_Y, "Y (distance)"); // label y axis
 
plot.data2DCurve(timedata, distances, numDataPoints); // plot all data to graph
plot.plotting(); // pull up plot