make it actually work now

src/main.cpp

@@ -10,22 +10,23 @@
 // ---- START VEXCODE CONFIGURED DEVICES ----
 // Robot Configuration:
 // [Name]               [Type]        [Port(s)]
-// Drivetrain           drivetrain    1, 2           
+// Drivetrain           drivetrain    1, 2
-// LiftMotor            motor         18     
+// LiftMotor            motor         18
-// LiftMotor2           motor         12           
+// LiftMotor2           motor         12
-// TiltMotor            motor         3               
+// TiltMotor            motor         3
-// Controller1          controller                    
+// Controller1          controller
-// IntakeR              motor         17               
+// IntakeR              motor         17
-// IntakeL              motor         8               
+// IntakeL              motor         8
 // ---- END VEXCODE CONFIGURED DEVICES ----
 
 #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();
@@ -104,80 +107,32 @@ void autonomous(void) {
 
   LiftMotor.setStopping(hold);
   LiftMotor2.setStopping(hold);
-  //STOPCODE_DEBUG();
+  // STOPCODE_DEBUG();
-  //uint8_t * instructions = nullptr;
+  // 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;
-  while(std::getline(ifs, instructions)) {
+  tim.clear();
-    int y = 0;
+  while (std::getline(ifs, instructions)) {
-    y = instructions.find(" ", 5);
+    while (tim.value() < 0.075) {
-    double vel = strtod(instructions.substr(0, y).c_str(), NULL);
+      wait(1, msec);
-    instructions = instructions.substr(y + 1);
+    }
-    y = instructions.find(" ", 5);
+    tim.clear();
-    double pos = strtod(instructions.substr(0, y).c_str(), NULL);
+    playback();
-    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();
   Drivetrain.setDriveVelocity(40, percent);
   Drivetrain.setTurnVelocity(50, percent);
- // Drivetrain.turnFor(5, degrees); 
+  // Drivetrain.turnFor(5, degrees);
   Drivetrain.driveFor(15, inches, false);
   wait(2, sec);
   Drivetrain.setDriveVelocity(80, percent);
   Drivetrain.driveFor(reverse, 16, inches);
   wait(1, sec);
-  //Drivetrain.turnFor(-45, degrees); 
+  // Drivetrain.turnFor(-45, degrees);
   STOPCODE_DEBUG();
   LiftMotor.startSpinTo(40 * 7, degrees); // unlock everything
   LiftMotor2.startSpinTo(40 * 7, degrees);
@@ -188,22 +143,23 @@ void autonomous(void) {
   waitUntil(LiftMotor.isDone() && LiftMotor2.isDone() && TiltMotor.isDone());
   IntakeL.setVelocity(100, percent);
   IntakeR.setVelocity(100, percent);
-  IntakeL.startSpinTo(-5, rev); 
+  IntakeL.startSpinTo(-5, rev);
   IntakeR.startSpinTo(-5, rev);
   TiltMotor.startSpinTo(5, degrees);
   waitUntil(TiltMotor.isDone());
-  //STOPCODE_DEBUG();
+  // STOPCODE_DEBUG();
   STOPCODE_DEBUG();
   IntakeL.setVelocity(100, percent);
   IntakeR.setVelocity(100, percent);
-  //IntakeL.startSpinTo(5, rev); // grab preload
+  // IntakeL.startSpinTo(5, rev); // grab preload
-  //IntakeR.startSpinTo(5, rev);
+  // IntakeR.startSpinTo(5, rev);
   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
+  // Drivetrain.turnFor(-90, degrees); // turn LEFT
   IntakeL.spin(forward);
   IntakeR.spin(forward);
   Drivetrain.setDriveVelocity(50, percent);
@@ -217,16 +173,62 @@ void autonomous(void) {
   Drivetrain.turnFor(130, degrees); // turn RIGHT towards corner
   Drivetrain.setDriveVelocity(33, percent);
   Drivetrain.driveFor(24 * 0.8, inches); // go to corner
-  TiltMotor.spinTo(50 * 7, degrees); // tilt cubes
+  TiltMotor.spinTo(50 * 7, degrees);     // tilt cubes
   IntakeL.setVelocity(20, percent);
   IntakeR.setVelocity(20, percent);
   IntakeL.startSpinFor(-3, rev);
   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.
@@ -260,133 +261,139 @@ void usercontrol(void) {
     // Insert user code here. This is where you use the joystick values to
     // update your motors, etc.
     // ........................................................................
-    if(Controller1.ButtonY.pressing()) {
+    if (Controller1.ButtonY.pressing()) {
       speedin = false;
       driveMultiplier = 0.33;
       Controller1.Screen.setCursor(0, 0);
       Controller1.Screen.clearLine();
       Controller1.Screen.print("Precise Driving ");
-    } else if(Controller1.ButtonX.pressing()) {
+    } else if (Controller1.ButtonX.pressing()) {
       speedin = true;
       driveMultiplier = 1;
-      Controller1.Screen.setCursor(0, 0); 
+      Controller1.Screen.setCursor(0, 0);
       Controller1.Screen.clearLine();
       Controller1.Screen.print("Normal Driving  ");
     }
-    if(Controller1.ButtonA.pressing() && !apressed) {
+    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 (Controller1.ButtonB.pressing() && !bpressed) {
-      if(driveMultiplier == 1.0) {
+      if (driveMultiplier == 1.0) {
         driveMultiplier = 0.33;
         Controller1.Screen.setCursor(0, 0);
-        //Controller1.Screen.clearLine();
+        // 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.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);
+    // uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5);
-    if(Controller1.ButtonLeft.pressing() && !recording) {
+    if (Controller1.ButtonLeft.pressing() && !recording) {
       recording = true;
 
-      LeftDriveSmart.resetRotation();      
+      LeftDriveSmart.resetRotation();
       RightDriveSmart.resetRotation();
       LiftMotor.resetRotation();
       LiftMotor2.resetRotation();
       TiltMotor.resetRotation();
-      TiltMotor2.resetRotation();
       IntakeL.resetRotation();
       IntakeR.resetRotation();
       if (Brain.SDcard.isInserted()) {
-      // create a file with long filename
+        // create a file with long filename
         ofs.open("recording.txt", std::ofstream::out);
       }
+      tim2.clear();
     }
-    if(Controller1.ButtonRight.pressing()) {
+    if (Controller1.ButtonRight.pressing()) {
       donerecording = true;
     }
-    if(recording) {
+    controllerBaseInput();
-      //count ++;
+    if (recording) {
+      while (tim2.value() < 0.075) {
+        wait(1, msec);
+      }
+      tim2.clear();
+      // count ++;
       std::string thisReading = "";
       char output[12];
       snprintf(output, 12, "%f", LeftDriveSmart.velocity(percent));
       thisReading += output;
       thisReading += " ";
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", LeftDriveSmart.rotation(degrees));
       thisReading += output;
       thisReading += " ";
 
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", RightDriveSmart.velocity(percent));
       thisReading += output;
       thisReading += " ";
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", RightDriveSmart.rotation(degrees));
       thisReading += output;
       thisReading += " ";
 
-      //char output[12];
+      // char output[12];
       snprintf(output, 12, "%f", IntakeL.velocity(percent));
       thisReading += output;
       thisReading += " ";
 
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", LiftMotor.velocity(percent));
       thisReading += output;
       thisReading += " ";
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", LiftMotor.rotation(degrees));
       thisReading += output;
       thisReading += " ";
 
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", TiltMotor.velocity(percent));
       thisReading += output;
       thisReading += " ";
-      //output[12] = "";
+      // output[12] = "";
       snprintf(output, 12, "%f", TiltMotor.rotation(degrees));
       thisReading += output;
       thisReading += " \n";
       ofs << thisReading;
-      //wait(30, msec);
+
-      //data = (uint8_t*)realloc(data, sizeof(uint8_t) * 5 * count + 5);
+      // data = (uint8_t*)realloc(data, sizeof(uint8_t) * 5 * count + 5);
-      //uint8_t* thisReading;
+      // uint8_t* thisReading;
-      //thisReading = new uint8_t[5];
+      // thisReading = new uint8_t[5];
-      /*thisReading = {(uint8_t) (LeftDriveSmart.velocity(percent)+100), 
+      /*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);
       //*(thisReading+2) = (uint8_t) (IntakeL.velocity(percent)+100);
       //*(thisReading+3) = (uint8_t) (LiftMotor.velocity(percent)+100);
       //*(thisReading+4) = (uint8_t) (TiltMotor.velocity(percent)+100);
-      //int startIndex = count * 5;
+      // int startIndex = count * 5;
-      //for(int i = 0; i < 5; i++) {
+      // for(int i = 0; i < 5; i++) {
       //  data[startIndex + i] = thisReading[i];
       //}
-      if(donerecording) {
+      if (donerecording) {
-        //Brain.SDcard.savefile("recording.bin", data, sizeof(data));
+        // Brain.SDcard.savefile("recording.bin", data, sizeof(data));
         recording = false;
         ofs.close();
       }
+    } else {
+      wait(20, msec); // Sleep the task for a short amount of time to
+                      // prevent wasted resources.
     }
-    controllerBaseInput();
+    // controllerBaseInput();
-    wait(50, msec); // Sleep the task for a short amount of time to
-                    // prevent wasted resources.
   }
 }
 
@@ -413,48 +420,87 @@ 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 drivetrainLeftSideSpeed = Controller1.Axis3.position();
-  //int drivetrainRightSideSpeed = Controller1.Axis2.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;
+  // if(Controller1.Axis4.position() < 0) axisAngle += 180;
-  //Controller1.Screen.setCursor(0, 50);
+  // Controller1.Screen.setCursor(0, 50);
-  //Controller1.Screen.clearLine();
+  // Controller1.Screen.clearLine();
-  //Controller1.Screen.print(axisAngle);
+  // 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)
-  if(axisAngle <= 181 && axisAngle >= 90) drivetrainRightSideSpeed = powerLevel;
+    drivetrainLeftSideSpeed = -powerLevel;
-  else if(axisAngle < 90 && axisAngle >= 0) drivetrainRightSideSpeed = -int(powerLevel * (45 - axisAngle) / 45);
+  else if (axisAngle > -90 && axisAngle < 0)
-  else if(axisAngle > -90 && axisAngle < 0) drivetrainRightSideSpeed = -powerLevel;
+    drivetrainLeftSideSpeed = int(powerLevel * (45 + axisAngle) / 45);
-  else if(axisAngle >= -181 && axisAngle <= -90) drivetrainRightSideSpeed = -int(powerLevel * (135 + axisAngle) / 45);
+
-  
+  if (axisAngle <= 181 && axisAngle >= 90)
-  if(Controller1.Axis4.position() == 0 && Controller1.Axis3.position() > 0 && (int(axisAngle) == 90 || int(axisAngle) == 0)) {
+    drivetrainRightSideSpeed = powerLevel;
+  else if (axisAngle < 90 && axisAngle >= 0)
+    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)) {
     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;
   }
-  int IntakeSpeed = Controller1.Axis2.position(); 
+  int IntakeSpeed = Controller1.Axis2.position();
   if (IntakeSpeed < 5 && IntakeSpeed > -5) {
     // check if the left motor has already been stopped
     if (IntakeStop) {
@@ -465,34 +511,34 @@ 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
-  if(speedin) {
+  // deadband range
+  if (speedin) {
     Controller1.Screen.setCursor(2, 0);
-    //Controller1.Screen.clearLine();
+    // Controller1.Screen.clearLine();
     Controller1.Screen.print("Fast intake");
   } else {
     Controller1.Screen.setCursor(2, 0);
-    //Controller1.Screen.clearLine();
+    // Controller1.Screen.clearLine();
     Controller1.Screen.print("Slow intake");
     IntakeSpeed /= 4;
   }
 
-  if(Controller1.ButtonDown.pressing()) {
+  if (Controller1.ButtonDown.pressing()) {
     drivetrainLeftSideSpeed = -20;
     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,12 +600,14 @@ 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()) {
     IntakeR.spin(forward);
@@ -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){
+  } 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;
   }
 }