d

src/main.cpp

@@ -41,6 +41,7 @@ drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295,
 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);
@@ -95,6 +96,7 @@ void autonomous(void) {
   LiftMotor.resetRotation();
   LiftMotor2.resetRotation();
   TiltMotor.resetRotation();
+  TiltMotor2.resetRotation();
   IntakeL.resetRotation();
   IntakeR.resetRotation();
   LeftDriveSmart.resetRotation();
@@ -118,7 +120,13 @@ void autonomous(void) {
     instructions = instructions.substr(y + 1);
     Controller1.Screen.print(vel);
     LeftDriveSmart.setVelocity(vel, percent);
-    LeftDriveSmart.startSpinTo(pos, degrees);
+    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);
@@ -128,7 +136,12 @@ void autonomous(void) {
     instructions = instructions.substr(y + 1);
     Controller1.Screen.print(vel);
     RightDriveSmart.setVelocity(vel, percent);
-    RightDriveSmart.startSpinTo(pos, degrees);
+    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);
@@ -151,6 +164,7 @@ void autonomous(void) {
     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();
@@ -228,6 +242,7 @@ 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();
@@ -289,6 +304,7 @@ void usercontrol(void) {
       LiftMotor.resetRotation();
       LiftMotor2.resetRotation();
       TiltMotor.resetRotation();
+      TiltMotor2.resetRotation();
       IntakeL.resetRotation();
       IntakeR.resetRotation();
       if (Brain.SDcard.isInserted()) {
@@ -343,7 +359,7 @@ 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];
@@ -369,7 +385,7 @@ void usercontrol(void) {
       }
     }
     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.
   }
 }
@@ -397,6 +413,7 @@ void STOPCODE_DEBUG() {
   IntakeR.stop();
   IntakeL.stop();
   TiltMotor.stop();
+  TiltMotor2.stop();
   while(true) wait(20, msec);
 }
 
@@ -557,12 +574,15 @@ void controllerBaseInput() {
   // 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 the buttons are released
     Controller1UpDownButtonsControlMotorsStopped = true;
   }