update superstructure

- implement new intake control (tasks)
- update constants
- add constant hold voltage to intake

CLOUDS/include/superstructure.hpp

@@ -3,6 +3,7 @@
 #define DRIVE_SPEED 127
 #define TURN_SPEED 120
 #define SWING_SPEED 120
+
 // R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE
 // Renu's control preferences
 #define RENU_PTO_TOGGLE DIGITAL_R1
@@ -30,6 +31,7 @@ namespace superstruct {
     void motorsCoast();
     void motorsHold();
     void motorsBrake();
+    void controllerTelemtry();
 
     void disableActiveBrake();
     
@@ -51,7 +53,7 @@ namespace superstruct {
     /* Structure */
     void togglePto(bool toggle);
     void runCata(double inpt);
-    void runIntake(double inpt);
+    void setIntakeSpeed(double inpt);
     void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton);
     void wingsControl(pros::controller_digital_e_t wingControlButton);
     void actuateClimb();

CLOUDS/src/superstructure.cpp

@@ -16,8 +16,12 @@ double speedScale = 1.0;
 double turnScale = 1.0;
 double swingScale = 1.0;
 
-namespace superstruct {
+double intakeSpeed = 0;
-  void chassisInit() {
+
+namespace superstruct
+{
+  void chassisInit()
+  {
     /*
       When the robot first starts up we want to do a couple things:
       - Adjust the drivetrain curve bottons so it does not interfere with any of the driver controls.
@@ -37,21 +41,24 @@ namespace superstruct {
     chassis.set_joystick_turnscale(1.0);
   }
 
-  void opControlInit() {
+  void opControlInit()
+  {
     motorsCoast();
-	  disableActiveBrake();
+    disableActiveBrake();
   }
 
   // Adjust exit conditions to allow for quick movements
-  void configureExitConditions() {
+  void configureExitConditions()
-    chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 150, 500);
+  {
-    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 150, 500);
+    chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);
-    chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 150, 500);
+    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
+    chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);
   }
 
   // Adjust PID constants for accurate movements
-  void configureConstants() {
+  void configureConstants()
-    chassis.set_slew_min_power(100, 100);
+  {
+    chassis.set_slew_min_power(80, 80);
     chassis.set_slew_distance(7, 7);
     chassis.set_pid_constants(&chassis.headingPID, 16, 0, 32, 0);
     chassis.set_pid_constants(&chassis.forward_drivePID, 0.5, 0, 5, 0);
@@ -61,69 +68,103 @@ namespace superstruct {
   }
 
   // Prepare the bot for the autonomous period of a match
-  void autonomousResets() {
+  void autonomousResets()
+  {
     chassis.reset_pid_targets();
-  	chassis.reset_gyro();
+    chassis.reset_gyro();
-  	chassis.reset_drive_sensor();
+    chassis.reset_drive_sensor();
     configureConstants();
     configureExitConditions();
     motorsBrake();
-    
   }
 
-  void motorsCoast() {
+  void motorsCoast()
+  {
     chassis.set_drive_brake(MOTOR_BRAKE_COAST);
   }
 
-  void motorsHold() {
+  void motorsHold()
+  {
     chassis.set_drive_brake(MOTOR_BRAKE_HOLD);
   }
 
-  void motorsBrake() {
+  void motorsBrake()
+  {
     chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
   }
 
   // The chassis will not apply a constant voltage to prevent it from being moved
-  void disableActiveBrake() {
+  void disableActiveBrake()
+  {
     chassis.set_active_brake(0.0);
   }
 
+  void handleIntake() {
+    while (true) {
+      intake_mtr.move_voltage(intakeSpeed);
+    }
+  }
+
+  pros::Task intakeManager(handleIntake);
+
+  // void controllerTelemetry()
+  // {
+  //   while (1)
+  //   {
+  //     for (int i = 0; i < 4; i++)
+  //     {
+  //       double localavg_one = 0;
+  //     }
+
+  //     for (int i = 0; i < 4; i++)
+  //     {
+  //       double 
+  //     }
+  //   }
+  // }
 
   // Drives forward, runs next commands WITHOUT waiting for the drive to complete
-  void driveAsync(double dist, bool useHeadingCorrection) {
+  void driveAsync(double dist, bool useHeadingCorrection)
+  {
     chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
   }
 
   // Drives forward, runs next commands AFTER waiting for the drive to complete
-  void driveSync(double dist, bool useHeadingCorrection) {
+  void driveSync(double dist, bool useHeadingCorrection)
+  {
     chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
     chassis.wait_drive();
   }
 
   // Drives forward, runs next commands AFTER reaching a certain measurement/error along the path
-  void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist) {
+  void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist)
+  {
     chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection);
     chassis.wait_until(waitUntilDist);
   }
 
   // Turns the chassis, runs other commands after it has run.
-  void turnSync(double theta) {
+  void turnSync(double theta)
+  {
     chassis.set_turn(theta, TURN_SPEED * turnScale);
     chassis.wait_drive();
   }
 
   // Turns the chassis, runs other commands immediately after call
-  void turnAsync(double theta) {
+  void turnAsync(double theta)
+  {
     chassis.set_turn(theta, TURN_SPEED * turnScale);
   }
 
   // Moves only the right side of the chassis so it can make a left turn
-  void leftSwing(double theta) {
+  void leftSwing(double theta)
+  {
     chassis.set_swing(LEFT_SWING, theta, SWING_SPEED * swingScale);
   }
 
   // Moves only the left side of the chassis so it can make a right turn
-  void rightSwing(double theta) {
+  void rightSwing(double theta)
+  {
     chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingScale);
   }
 
@@ -131,66 +172,89 @@ namespace superstruct {
     Each of the scale values must be clamed between 0.1 - 1 (10% to 100%) to avoid saturation of motors.
   */
 
-  void setDriveScale(double val) {
+  void setDriveScale(double val)
+  {
     speedScale = std::clamp(val, 0.1, 1.0);
   }
 
-  void setTurnScale(double val) {
+  void setTurnScale(double val)
+  {
     turnScale = std::clamp(val, 0.1, 1.0);
   }
 
-  void setSwingScale(double val) {
+  void setSwingScale(double val)
+  {
     swingScale = std::clamp(val, 0.1, 1.0); //
   }
 
   // Structure methods
-  void togglePto(bool toggle) {
+  void togglePto(bool toggle)
+  {
     ptoEnabled = toggle;
     chassis.pto_toggle({intake_mtr, cata_mtr}, toggle); // Configure the listed PTO motors to whatever value toggle is.
     pto_piston.set_value(toggle);
 
-    if (toggle) {
+    if (toggle)
+    {
       intake_mtr.set_brake_mode(MOTOR_BRAKE_COAST);
       cata_mtr.set_brake_mode(MOTOR_BRAKE_COAST);
     }
   }
 
-  void runCata(double inpt) {
+  void runCata(double inpt)
-    if (!ptoEnabled) return;
+  {
+    if (!ptoEnabled)
+      return;
     cata_mtr.move_voltage(inpt);
   }
 
-  void runIntake(double inpt) {
+  void setIntakeSpeed(double inpt)
-    if (!ptoEnabled) return;
+  {
-    intake_mtr.move_voltage(inpt);
+    if (!ptoEnabled)
+      intakeSpeed = 0;
+    intakeSpeed = inpt;
   }
 
   int lock = 0;
-  void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton) {
+  void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton)
-    if (globals::master.get_digital(ptoToggleButton) && lock == 0) { // If the PTO button has been pressed and the PTO is not engaged
+  {
+    if (globals::master.get_digital(ptoToggleButton) && lock == 0)
+    {                         // If the PTO button has been pressed and the PTO is not engaged
       togglePto(!ptoEnabled); // Toggle the PTO so that cataput is useable
       lock = 1;
-    } else if(!globals::master.get_digital(ptoToggleButton)) {
+    }
+    else if (!globals::master.get_digital(ptoToggleButton))
+    {
       lock = 0;
     }
 
-    if (globals::master.get_digital(cataRunButton)) {
+    if (globals::master.get_digital(cataRunButton))
-			runCata(-12000);
+    {
-		} else {
+      runCata(-12000);
-			runCata(0);
+    }
-		}
+    else
+    {
+      runCata(0);
+    }
 
-    if (globals::master.get_digital(intakeButton)) {
+    if (globals::master.get_digital(intakeButton))
-      runIntake(-12000);
+    {
-    } else if (globals::master.get_digital(outtakeButton)) {
+      setIntakeSpeed(-12000);
-      runIntake(12000);
+    }
-    } else {
+    else if (globals::master.get_digital(outtakeButton))
-      runIntake(0);
+    {
+      setIntakeSpeed(12000);
+    }
+    else
+    {
+      setIntakeSpeed(-1000);
     }
   }
 
-  void wingsControl(pros::controller_digital_e_t wingControlButton) {
+  void wingsControl(pros::controller_digital_e_t wingControlButton)
-    if (globals::master.get_digital_new_press(wingControlButton)) {
+  {
+    if (globals::master.get_digital_new_press(wingControlButton))
+    {
       if (wings.getState() == 0) // A value of 0 indicates that both wings are closed
         wings.open();
       else if (wings.getState() == 3) // A value of 3 indicates that both wings are open
@@ -198,13 +262,16 @@ namespace superstruct {
     }
   }
 
-  void actuateClimb() {
+  void actuateClimb()
+  {
     climb_piston_one.set_value(1);
     climb_piston_two.set_value(1);
   }
 
-  void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2) {
+  void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2)
-    if (globals::master.get_digital(but1) && globals::master.get_digital(but2)) {
+  {
+    if (globals::master.get_digital(but1) && globals::master.get_digital(but2))
+    {
       actuateClimb();
     }
   }
@@ -212,19 +279,22 @@ namespace superstruct {
   /*
     Controls -> For whoever is controlling the robot
   */
-  void renu_control() {
+  void renu_control()
+  {
     subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
     wingsControl(RENU_WING_CONTROL);
     climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
   }
 
-  void ria_control() {
+  void ria_control()
+  {
     subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
     wingsControl(RENU_WING_CONTROL);
     climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
   }
 
-  void chris_control() {
+  void chris_control()
+  {
     subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
     wingsControl(RENU_WING_CONTROL);
     climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);