Add intake and drive controls for stacking

src/main.cpp

@@ -10,12 +10,13 @@
 // ---- START VEXCODE CONFIGURED DEVICES ----
 // Robot Configuration:
 // [Name]               [Type]        [Port(s)]
-// Drivetrain           drivetrain    10, 1           
-// LiftMotor            motor         9               
-// TiltMotor            motor         8               
+// Drivetrain           drivetrain    1, 2           
+// LiftMotor            motor         18     
+// LiftMotor2           motor         12           
+// TiltMotor            motor         3               
 // Controller1          controller                    
-// IntakeR              motor         2               
-// IntakeL              motor         3               
+// IntakeR              motor         17               
+// IntakeL              motor         8               
 // ---- END VEXCODE CONFIGURED DEVICES ----
 
 #include "vex.h"
@@ -231,7 +232,6 @@ void usercontrol(void) {
   Controller1.Screen.print("Normal Driving ");
   bool recording = false;
   bool donerecording = false;
-  //uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5);
   
   // User control code here, inside the loop
   while (1) {
@@ -435,18 +435,54 @@ void controllerBaseInput() {
     drivetrainLeftSideSpeed = -powerLevel;
     drivetrainRightSideSpeed = powerLevel;
   }
+  int IntakeSpeed = Controller1.Axis2.position(); 
+  if (IntakeSpeed < 5 && IntakeSpeed > -5) {
+    // check if the left motor has already been stopped
+    if (IntakeStop) {
+      // stop the left drive motor
+      IntakeL.stop();
+      IntakeR.stop();
+      // tell the code that the left motor has been stopped
+      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
+    IntakeStop = true;
+  }
+  // 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();
+    Controller1.Screen.print("Fast intake");
+  } else {
+    Controller1.Screen.setCursor(2, 0);
+    //Controller1.Screen.clearLine();
+    Controller1.Screen.print("Slow intake");
+    IntakeSpeed /= 4;
+  }
+
   if(Controller1.ButtonDown.pressing()) {
-    drivetrainLeftSideSpeed = -33;
-    drivetrainRightSideSpeed = -33;
+    drivetrainLeftSideSpeed = -20;
+    drivetrainRightSideSpeed = -20;
+    IntakeStop = true;
+    IntakeSpeed = -20;
   }
   else if(Controller1.ButtonUp.pressing()) {
-    drivetrainLeftSideSpeed = 33;
-    drivetrainRightSideSpeed = 33;
+    drivetrainLeftSideSpeed = 20;
+    drivetrainRightSideSpeed = 20;
+    IntakeStop = true;
+    IntakeSpeed = 20;
   } 
   else {
     drivetrainLeftSideSpeed *= driveMultiplier;
     drivetrainRightSideSpeed *= driveMultiplier;
   }
+  if (IntakeStop) {
+    IntakeL.setVelocity(IntakeSpeed, percent);
+    IntakeR.setVelocity(IntakeSpeed, percent);
+    IntakeL.spin(forward);
+    IntakeR.spin(forward);
+  }
   // check if the value is inside of the deadband range
   if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
     // check if the left motor has already been stopped
@@ -498,37 +534,7 @@ void controllerBaseInput() {
     // set the toggle so that we don't constantly tell the motor to stop when the buttons are released
     Controller1LeftShoulderControlMotorsStopped = true;
   }
-  int IntakeSpeed = Controller1.Axis2.position(); 
-  if (IntakeSpeed < 5 && IntakeSpeed > -5) {
-    // check if the left motor has already been stopped
-    if (IntakeStop) {
-      // stop the left drive motor
-      IntakeL.stop();
-      IntakeR.stop();
-      // tell the code that the left motor has been stopped
-      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
-    IntakeStop = true;
-  }
-  // 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();
-    Controller1.Screen.print("Fast intake");
-  } else {
-    Controller1.Screen.setCursor(2, 0);
-    //Controller1.Screen.clearLine();
-    Controller1.Screen.print("Slow intake");
-    IntakeSpeed /= 4;
-  }
-  if (IntakeStop) {
-    IntakeL.setVelocity(IntakeSpeed, percent);
-    IntakeR.setVelocity(IntakeSpeed, percent);
-    IntakeL.spin(forward);
-    IntakeR.spin(forward);
-  }
+  
   // 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