make it actually work now

src/main.cpp

@@ -21,11 +21,12 @@
 
 #include "vex.h"
 #include <cstdio>
-#include <iostream>
-#include <iomanip>
 #include <fstream>
+#include <iomanip>
+#include <iostream>
 #include <string>
 
+
 using namespace vex;
 bool speedin = true;
 double driveMultiplier = 1.0;
@@ -37,11 +38,11 @@ competition Competition;
 
 motor LeftDriveSmart = motor(PORT1, ratio18_1, false);
 motor RightDriveSmart = motor(PORT2, ratio18_1, true);
-drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 130, mm, 1);
+drivetrain Drivetrain =
+    drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 130, mm, 1);
 motor LiftMotor = motor(PORT18, ratio18_1, true);
 motor LiftMotor2 = motor(PORT12, ratio18_1, false);
 motor TiltMotor = motor(PORT3, ratio18_1, false);
-motor TiltMotor2 = motor(PORT10, ratio18_1, true);
 controller Controller1 = controller(primary);
 motor IntakeR = motor(PORT17, ratio18_1, false);
 motor IntakeL = motor(PORT8, ratio18_1, true);
@@ -84,6 +85,9 @@ void pre_auton(void) {
 /*  You must modify the code to add your own robot specific commands here.   */
 /*---------------------------------------------------------------------------*/
 std::ifstream ifs;
+void goToPos(motor m, double vel, double pos);
+void playback();
+std::string instructions;
 void autonomous(void) {
   // ..........................................................................
   // Insert autonomous user code here.
@@ -96,7 +100,6 @@ void autonomous(void) {
   LiftMotor.resetRotation();
   LiftMotor2.resetRotation();
   TiltMotor.resetRotation();
-  TiltMotor2.resetRotation();
   IntakeL.resetRotation();
   IntakeR.resetRotation();
   LeftDriveSmart.resetRotation();
@@ -106,66 +109,18 @@ void autonomous(void) {
   LiftMotor2.setStopping(hold);
   // STOPCODE_DEBUG();
   // uint8_t * instructions = nullptr;
-  //Brain.SDcard.loadfile("recording.bin", instructions, 2500); // size in bytes
+  // Brain.SDcard.loadfile("recording.bin", instructions, 2500); // size in
+  // bytes
   ifs.open("recording.txt", std::ofstream::in);
   Controller1.Screen.print("File opened");
-  std::string instructions;
+  timer tim;
+  tim.clear();
   while (std::getline(ifs, instructions)) {
-    int y = 0;
+    while (tim.value() < 0.075) {
-    y = instructions.find(" ", 5);
+      wait(1, msec);
-    double vel = strtod(instructions.substr(0, y).c_str(), NULL);
+    }
-    instructions = instructions.substr(y + 1);
+    tim.clear();
-    y = instructions.find(" ", 5);
+    playback();
-    double pos = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    Controller1.Screen.print(vel);
-    LeftDriveSmart.setVelocity(vel, percent);
-    if(LeftDriveSmart.rotation(degrees) < pos && vel >  0)
-      LeftDriveSmart.spin(forward);
-    else if(LeftDriveSmart.rotation(degrees) > pos && vel < 0) 
-      LeftDriveSmart.spin(forward);
-    else 
-      LeftDriveSmart.stop();
-    //LeftDriveSmart.startSpinTo(pos, degrees);
-    
-    y = instructions.find(" ", 5);
-    vel = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    y = instructions.find(" ", 5);
-    pos = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    Controller1.Screen.print(vel);
-    RightDriveSmart.setVelocity(vel, percent);
-    if(RightDriveSmart.rotation(degrees) < pos && vel >  0)
-      RightDriveSmart.spin(forward);
-    else if(RightDriveSmart.rotation(degrees) > pos && vel < 0) 
-      RightDriveSmart.spin(forward);
-    else 
-      RightDriveSmart.stop();
-
-    y = instructions.find(" ", 5);
-    vel = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    IntakeL.spin(forward, vel, percent);
-    IntakeR.spin(forward, vel, percent);
-
-    y = instructions.find(" ", 5);
-    vel = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    y = instructions.find(" ", 5);
-    pos = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    LiftMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
-    LiftMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
-
-    y = instructions.find(" ", 5);
-    vel = strtod(instructions.substr(0, y).c_str(), NULL);
-    instructions = instructions.substr(y + 1);
-    y = instructions.find(" ", 5);
-    pos = strtod(instructions.substr(0, y).c_str(), NULL);
-    TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
-    TiltMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
-    wait(10, msec);
   }
   ifs.close();
   STOPCODE_DEBUG();
@@ -201,7 +156,8 @@ void autonomous(void) {
   Drivetrain.setDriveVelocity(80, percent);
   Drivetrain.setTurnVelocity(100, percent);
 
-  //Drivetrain.driveFor(24 * 3.5, inches); // drive to the row of 4 (if on that tile)
+  // Drivetrain.driveFor(24 * 3.5, inches); // drive to the row of 4 (if on that
+  // tile)
   waitUntil(IntakeL.isDone() && IntakeR.isDone());
   // Drivetrain.turnFor(-90, degrees); // turn LEFT
   IntakeL.spin(forward);
@@ -224,9 +180,55 @@ void autonomous(void) {
   IntakeR.startSpinFor(-3, rev); // start pushing out cubes
   Drivetrain.setDriveVelocity(25, percent);
   Drivetrain.driveFor(reverse, 24 * 0.8, inches); // back away slowly
-
 }
+void playback() {
+  int y = 0;
+  y = instructions.find(" ", 5);
+  double vel = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  y = instructions.find(" ", 5);
+  double pos = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  Controller1.Screen.print(vel);
+  
+  // LeftDriveSmart.setVelocity(vel, percent);
+  // LeftDriveSmart.startSpinTo(pos, degrees);
+
+  y = instructions.find(" ", 5);
+  double vel2 = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  y = instructions.find(" ", 5);
+  double pos2 = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  //Controller1.Screen.print(vel);
+  goToPos(LeftDriveSmart, vel, pos);
+  goToPos(RightDriveSmart, vel2, pos2);
+
+  y = instructions.find(" ", 5);
+  vel = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  IntakeL.spin(forward, vel, percent);
+  IntakeR.spin(forward, vel, percent);
+
+  y = instructions.find(" ", 5);
+  vel = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  y = instructions.find(" ", 5);
+  pos = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  goToPos(LiftMotor, vel, pos);
+  goToPos(LiftMotor2, vel, pos);
+  //LiftMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
 
+  y = instructions.find(" ", 5);
+  vel = strtod(instructions.substr(0, y).c_str(), NULL);
+  instructions = instructions.substr(y + 1);
+  y = instructions.find(" ", 5);
+  pos = strtod(instructions.substr(0, y).c_str(), NULL);
+  goToPos(TiltMotor, vel, pos);
+  //TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
+  //wait(10, msec);
+}
 /*---------------------------------------------------------------------------*/
 /*                                                                           */
 /*                              User Control Task                            */
@@ -242,14 +244,13 @@ void usercontrol(void) {
   LiftMotor.setVelocity(100, percent);
   LiftMotor2.setVelocity(100, percent);
   TiltMotor.setVelocity(30, percent);
-  TiltMotor2.setVelocity(30, percent);
   Controller1.Screen.clearScreen();
   Controller1.Screen.setCursor(0, 0);
   Controller1.Screen.clearLine();
   Controller1.Screen.print("Normal Driving ");
   bool recording = false;
   bool donerecording = false;
-  
+  timer tim2;
   // User control code here, inside the loop
   while (1) {
     // This is the main execution loop for the user control program.
@@ -276,8 +277,8 @@ void usercontrol(void) {
     if (Controller1.ButtonA.pressing() && !apressed) {
       speedin = !speedin;
       apressed = true;
-    }
+    } else if (!Controller1.ButtonA.pressing())
-    else if(!Controller1.ButtonA.pressing()) apressed = false;
+      apressed = false;
 
     if (Controller1.ButtonB.pressing() && !bpressed) {
       if (driveMultiplier == 1.0) {
@@ -285,16 +286,15 @@ void usercontrol(void) {
         Controller1.Screen.setCursor(0, 0);
         // Controller1.Screen.clearLine();
         Controller1.Screen.print("Precise Driving");
-      }
+      } else if (driveMultiplier == 0.33) {
-      else if(driveMultiplier == 0.33) {
         driveMultiplier = 1;
         Controller1.Screen.setCursor(0, 0);
         // Controller1.Screen.clearLine();
         Controller1.Screen.print("Normal Driving ");
       }
       bpressed = true;
-    }
+    } else if (!Controller1.ButtonB.pressing())
-    else if(!Controller1.ButtonB.pressing()) bpressed = false;
+      bpressed = false;
     // uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5);
     if (Controller1.ButtonLeft.pressing() && !recording) {
       recording = true;
@@ -304,18 +304,23 @@ void usercontrol(void) {
       LiftMotor.resetRotation();
       LiftMotor2.resetRotation();
       TiltMotor.resetRotation();
-      TiltMotor2.resetRotation();
       IntakeL.resetRotation();
       IntakeR.resetRotation();
       if (Brain.SDcard.isInserted()) {
         // create a file with long filename
         ofs.open("recording.txt", std::ofstream::out);
       }
+      tim2.clear();
     }
     if (Controller1.ButtonRight.pressing()) {
       donerecording = true;
     }
+    controllerBaseInput();
     if (recording) {
+      while (tim2.value() < 0.075) {
+        wait(1, msec);
+      }
+      tim2.clear();
       // count ++;
       std::string thisReading = "";
       char output[12];
@@ -359,15 +364,16 @@ void usercontrol(void) {
       thisReading += output;
       thisReading += " \n";
       ofs << thisReading;
-      //wait(30, msec);
+
       // data = (uint8_t*)realloc(data, sizeof(uint8_t) * 5 * count + 5);
       // uint8_t* thisReading;
       // thisReading = new uint8_t[5];
       /*thisReading = {(uint8_t) (LeftDriveSmart.velocity(percent)+100),
-                                (uint8_t) (RightDriveSmart.velocity(percent)+100),
+                                (uint8_t)
-                                (uint8_t) (IntakeL.velocity(percent)+100),
+      (RightDriveSmart.velocity(percent)+100), (uint8_t)
-                                (uint8_t) (LiftMotor.velocity(percent)+100),
+      (IntakeL.velocity(percent)+100), (uint8_t)
-                                (uint8_t) (TiltMotor.velocity(percent)+100)
+      (LiftMotor.velocity(percent)+100), (uint8_t)
+      (TiltMotor.velocity(percent)+100)
       };*/
       //*(thisReading) = (uint8_t) (LeftDriveSmart.velocity(percent)+100);
       //*(thisReading+1) = (uint8_t) (RightDriveSmart.velocity(percent)+100);
@@ -383,11 +389,12 @@ void usercontrol(void) {
         recording = false;
         ofs.close();
       }
-    }
+    } else {
-    controllerBaseInput();
+      wait(20, msec); // Sleep the task for a short amount of time to
-    wait(50, msec); // Sleep the task for a short amount of time to
                       // prevent wasted resources.
     }
+    // controllerBaseInput();
+  }
 }
 
 //
@@ -413,15 +420,40 @@ void STOPCODE_DEBUG() {
   IntakeR.stop();
   IntakeL.stop();
   TiltMotor.stop();
-  TiltMotor2.stop();
+  while (true)
-  while(true) wait(20, msec);
+    wait(20, msec);
+}
+
+/*void goToPos(motor m, double vel, double pos) {
+  m.setVelocity(vel, percent);
+  if (true || pos > m.rotation(degrees)) {
+    if (vel > 0 && !m.isSpinning()) {
+      m.spin(forward);
+    }
+  } else {
+    m.stop();
   }
 
+  if (true || pos < m.rotation(degrees)) {
+    if (vel < 0 && !m.isSpinning()) {
+      m.spin(forward);
+    }
+  } else {
+    m.stop();
+  }
+}*/
+void goToPos(motor m, double vel, double pos) {
+  m.setVelocity(vel, percent);
+  if (vel != 0 && !m.isSpinning()) {
+    m.spin(forward);
+  }
+}
 void controllerBaseInput() {
 
   // int drivetrainLeftSideSpeed = Controller1.Axis3.position();
   // int drivetrainRightSideSpeed = Controller1.Axis2.position();
-  double axisAngle = atan2(Controller1.Axis3.position(), Controller1.Axis4.position());
+  double axisAngle =
+      atan2(Controller1.Axis3.position(), Controller1.Axis4.position());
   axisAngle *= 180 / 3.1415926;
 
   // if(Controller1.Axis4.position() < 0) axisAngle += 180;
@@ -430,27 +462,41 @@ void controllerBaseInput() {
   // Controller1.Screen.print(axisAngle);
   int drivetrainLeftSideSpeed = Controller1.Axis3.position();
   int drivetrainRightSideSpeed = Controller1.Axis4.position();
-  int powerLevel = sqrt(Controller1.Axis3.position() * Controller1.Axis3.position() + Controller1.Axis4.position() * Controller1.Axis4.position());
+  int powerLevel =
+      sqrt(Controller1.Axis3.position() * Controller1.Axis3.position() +
+           Controller1.Axis4.position() * Controller1.Axis4.position());
   // left side
-  if(axisAngle < 90 && axisAngle >= 0) drivetrainLeftSideSpeed = powerLevel;
+  if (axisAngle < 90 && axisAngle >= 0)
-  else if(axisAngle <= 181 && axisAngle >= 90) drivetrainLeftSideSpeed = int(powerLevel * (135 - axisAngle) / 45);
+    drivetrainLeftSideSpeed = powerLevel;
-  else if(axisAngle >= -181 && axisAngle <= -90) drivetrainLeftSideSpeed = -powerLevel;
+  else if (axisAngle <= 181 && axisAngle >= 90)
-  else if(axisAngle > -90 && axisAngle < 0) drivetrainLeftSideSpeed = int(powerLevel * (45 + axisAngle) / 45);
+    drivetrainLeftSideSpeed = int(powerLevel * (135 - axisAngle) / 45);
+  else if (axisAngle >= -181 && axisAngle <= -90)
+    drivetrainLeftSideSpeed = -powerLevel;
+  else if (axisAngle > -90 && axisAngle < 0)
+    drivetrainLeftSideSpeed = int(powerLevel * (45 + axisAngle) / 45);
 
-  if(axisAngle <= 181 && axisAngle >= 90) drivetrainRightSideSpeed = powerLevel;
+  if (axisAngle <= 181 && axisAngle >= 90)
-  else if(axisAngle < 90 && axisAngle >= 0) drivetrainRightSideSpeed = -int(powerLevel * (45 - axisAngle) / 45);
+    drivetrainRightSideSpeed = powerLevel;
-  else if(axisAngle > -90 && axisAngle < 0) drivetrainRightSideSpeed = -powerLevel;
+  else if (axisAngle < 90 && axisAngle >= 0)
-  else if(axisAngle >= -181 && axisAngle <= -90) drivetrainRightSideSpeed = -int(powerLevel * (135 + axisAngle) / 45);
+    drivetrainRightSideSpeed = -int(powerLevel * (45 - axisAngle) / 45);
+  else if (axisAngle > -90 && axisAngle < 0)
+    drivetrainRightSideSpeed = -powerLevel;
+  else if (axisAngle >= -181 && axisAngle <= -90)
+    drivetrainRightSideSpeed = -int(powerLevel * (135 + axisAngle) / 45);
 
-  if(Controller1.Axis4.position() == 0 && Controller1.Axis3.position() > 0 && (int(axisAngle) == 90 || int(axisAngle) == 0)) {
+  if (Controller1.Axis4.position() == 0 && Controller1.Axis3.position() > 0 &&
+      (int(axisAngle) == 90 || int(axisAngle) == 0)) {
     drivetrainLeftSideSpeed = powerLevel;
     drivetrainRightSideSpeed = powerLevel;
   }
- if(Controller1.Axis4.position() == 0 && Controller1.Axis3.position() < 0 && (int(axisAngle) == -90 || int(axisAngle) == 0)) {
+  if (Controller1.Axis4.position() == 0 && Controller1.Axis3.position() < 0 &&
+      (int(axisAngle) == -90 || int(axisAngle) == 0)) {
     drivetrainLeftSideSpeed = -powerLevel;
     drivetrainRightSideSpeed = -powerLevel;
   }
-  if(Controller1.Axis4.position() < 0 && (int(axisAngle) == 180 || int(axisAngle) == -180 || int(axisAngle) == 0)) {
+  if (Controller1.Axis4.position() < 0 &&
+      (int(axisAngle) == 180 || int(axisAngle) == -180 ||
+       int(axisAngle) == 0)) {
     drivetrainLeftSideSpeed = -powerLevel;
     drivetrainRightSideSpeed = powerLevel;
   }
@@ -465,10 +511,12 @@ void controllerBaseInput() {
       IntakeStop = false;
     }
   } else {
-    // reset the toggle so that the deadband code knows to stop the left motor next time the input is in the deadband range
+    // reset the toggle so that the deadband code knows to stop the left motor
+    // next time the input is in the deadband range
     IntakeStop = true;
   }
-  // only tell the left drive motor to spin if the values are not in the deadband range
+  // only tell the left drive motor to spin if the values are not in the
+  // deadband range
   if (speedin) {
     Controller1.Screen.setCursor(2, 0);
     // Controller1.Screen.clearLine();
@@ -485,14 +533,12 @@ void controllerBaseInput() {
     drivetrainRightSideSpeed = -20;
     IntakeStop = true;
     IntakeSpeed = -20;
-  }
+  } else if (Controller1.ButtonUp.pressing()) {
-  else if(Controller1.ButtonUp.pressing()) {
     drivetrainLeftSideSpeed = 20;
     drivetrainRightSideSpeed = 20;
     IntakeStop = true;
     IntakeSpeed = 20;
-  } 
+  } else {
-  else {
     drivetrainLeftSideSpeed *= driveMultiplier;
     drivetrainRightSideSpeed *= driveMultiplier;
   }
@@ -512,7 +558,8 @@ void controllerBaseInput() {
       DrivetrainLNeedsToBeStopped_Controller1 = false;
     }
   } else {
-    // reset the toggle so that the deadband code knows to stop the left motor next time the input is in the deadband range
+    // reset the toggle so that the deadband code knows to stop the left motor
+    // next time the input is in the deadband range
     DrivetrainLNeedsToBeStopped_Controller1 = true;
   }
   // check if the value is inside of the deadband range
@@ -525,15 +572,18 @@ void controllerBaseInput() {
       DrivetrainRNeedsToBeStopped_Controller1 = false;
     }
   } else {
-    // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
+    // reset the toggle so that the deadband code knows to stop the right motor
+    // next time the input is in the deadband range
     DrivetrainRNeedsToBeStopped_Controller1 = true;
   }
-  // only tell the left drive motor to spin if the values are not in the deadband range
+  // only tell the left drive motor to spin if the values are not in the
+  // deadband range
   if (DrivetrainLNeedsToBeStopped_Controller1) {
     LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
     LeftDriveSmart.spin(forward);
   }
-  // only tell the right drive motor to spin if the values are not in the deadband range
+  // only tell the right drive motor to spin if the values are not in the
+  // deadband range
   if (DrivetrainRNeedsToBeStopped_Controller1) {
     RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
     RightDriveSmart.spin(forward);
@@ -550,11 +600,13 @@ void controllerBaseInput() {
   } else if (!Controller1LeftShoulderControlMotorsStopped) {
     LiftMotor.stop(hold);
     LiftMotor2.stop(hold);
-    // set the toggle so that we don't constantly tell the motor to stop when the buttons are released
+    // set the toggle so that we don't constantly tell the motor to stop when
+    // the buttons are released
     Controller1LeftShoulderControlMotorsStopped = true;
   }
 
-  // only tell the right drive motor to spin if the values are not in the deadband range
+  // only tell the right drive motor to spin if the values are not in the
+  // deadband range
 
   // check the ButtonR1/ButtonR2 status to control Intake
   /*if (Controller1.ButtonR1.pressing()) {
@@ -568,22 +620,20 @@ void controllerBaseInput() {
   } else if (!Controller1RightShoulderControlMotorsStopped) {
     IntakeR.stop(hold);
     IntakeL.stop(hold);
-    // set the toggle so that we don't constantly tell the motor to stop when the buttons are released
+    // set the toggle so that we don't constantly tell the motor to stop when
-    Controller1RightShoulderControlMotorsStopped = true;
+  the buttons are released Controller1RightShoulderControlMotorsStopped = true;
   }*/
   // check the Right Shoulder Buttons status to control TiltMotor
   if (Controller1.ButtonR1.pressing()) {
     TiltMotor.spin(forward);
-    TiltMotor2.spin(forward);
     Controller1UpDownButtonsControlMotorsStopped = false;
   } else if (Controller1.ButtonR2.pressing()) {
     TiltMotor.spin(reverse);
-    TiltMotor2.spin(reverse);
     Controller1UpDownButtonsControlMotorsStopped = false;
   } else if (!Controller1UpDownButtonsControlMotorsStopped) {
     TiltMotor.stop(hold);
-    TiltMotor2.stop(hold);
+    // set the toggle so that we don't constantly tell the motor to stop when
-    // set the toggle so that we don't constantly tell the motor to stop when the buttons are released
+    // the buttons are released
     Controller1UpDownButtonsControlMotorsStopped = true;
   }
 }