Rewrite traction control, stability control, and ABS with proper sensor-based implementations

src/comptition/comptition.cpp

@@ -1,5 +1,24 @@
 #include "vex.h"
 
+/**
+ * Advanced Drive Control Systems
+ * 
+ * This file implements traction control, stability control, and ABS systems
+ * for improved drive behavior during acceleration, turning, and uneven terrain.
+ * 
+ * Control Systems:
+ * - Traction Control: Detects and corrects wheel slip by monitoring motor velocities
+ * - Stability Control: Maintains directional stability using inertial sensor data
+ * - ABS: Prevents wheel lockup during hard braking
+ * 
+ * Controls:
+ * - Up: Drive mode menu
+ * - Down: Toggle debug output
+ * - L1: Toggle traction control
+ * - L2: Toggle stability control  
+ * - R1: Toggle ABS
+ */
+
 void autonomous()
 {
     logHandler("autonomous", "Test message.", Log::Level::Warn, 2);
@@ -16,6 +35,21 @@ bool tractionControlEnabled = true;
 bool stabilityControlEnabled = true;
 bool absEnabled = true;
 
+// Control system constants
+const double SLIP_THRESHOLD = 50.0;     // RPM difference threshold for slip detection
+const double TRACTION_REDUCTION = 0.8;   // Reduce power to 80% when slip detected
+const double HEADING_KP = 0.02;          // Proportional gain for heading correction
+const double MAX_HEADING_CORRECTION = 2.0; // Maximum voltage correction for heading
+const double ABS_VELOCITY_THRESHOLD = 300.0; // RPM drop threshold for ABS activation
+const double ABS_REDUCTION = 0.5;        // Reduce braking power to 50% during ABS
+const double ANTI_TIP_THRESHOLD = 15.0;  // Degrees of tilt before anti-tip activation
+
+// Static variables for control systems
+static double targetHeading = 0.0;
+static bool headingInitialized = false;
+static double previousVelocities[4] = {0.0, 0.0, 0.0, 0.0}; // FL, RL, FR, RR
+static bool debugOutput = false; // Enable debug printing
+
 /**
  * @author @DVT7125
  * @date 4/10/24
@@ -28,48 +62,218 @@ void displaySystemStates()
 {
     Brain.Screen.clearScreen();
     Brain.Screen.setCursor(1, 1);
-    Brain.Screen.print("Traction Control: %s\nStability Control: %s\nABS: %s",
+    Brain.Screen.print("Traction Control: %s\nStability Control: %s\nABS: %s\nDebug Output: %s",
                        tractionControlEnabled ? "ON" : "OFF",
                        stabilityControlEnabled ? "ON" : "OFF",
-                       absEnabled ? "ON" : "OFF");
+                       absEnabled ? "ON" : "OFF",
+                       debugOutput ? "ON" : "OFF");
 }
 
-// Function to apply traction control
-void applyTractionControl(double &forwardVolts)
+// Function to toggle debug output
+void toggleDebugOutput()
 {
-    double frontLeftMotorRPM = frontLeftMotor.velocity(vex::velocityUnits::rpm);
-    double rearLeftMotorRPM = rearLeftMotor.velocity(vex::velocityUnits::rpm);
-    double frontRightMotorRPM = frontRightMotor.velocity(vex::velocityUnits::rpm);
-    double rearRightMotorRPM = rearRightMotor.velocity(vex::velocityUnits::rpm);
-
-    double minSpeed = std::min({std::abs(frontLeftMotorRPM), std::abs(rearLeftMotorRPM), std::abs(frontRightMotorRPM), std::abs(rearRightMotorRPM)});
+    debugOutput = !debugOutput;
+    if (debugOutput) {
+        printf("Drive Control Debug Output: ENABLED\n");
+    } else {
+        printf("Drive Control Debug Output: DISABLED\n");
+    }
+}
 
-    forwardVolts = minSpeed;
+// Function to apply traction control
+void applyTractionControl(double &forwardVolts, double &turnVolts)
+{
+    // Get current motor velocities
+    double frontLeftRPM = frontLeftMotor.velocity(vex::velocityUnits::rpm);
+    double rearLeftRPM = rearLeftMotor.velocity(vex::velocityUnits::rpm);
+    double frontRightRPM = frontRightMotor.velocity(vex::velocityUnits::rpm);
+    double rearRightRPM = rearRightMotor.velocity(vex::velocityUnits::rpm);
+
+    // Calculate average velocities for left and right sides
+    double leftSideRPM = (frontLeftRPM + rearLeftRPM) / 2.0;
+    double rightSideRPM = (frontRightRPM + rearRightRPM) / 2.0;
+
+    // Calculate expected velocities based on commanded voltages
+    // Assuming linear relationship between voltage and RPM for simplicity
+    double expectedLeftRPM = (forwardVolts + turnVolts) * 10.0; // Scale factor
+    double expectedRightRPM = (forwardVolts - turnVolts) * 10.0;
+
+    // Detect slip by comparing expected vs actual velocities
+    double leftSlip = std::abs(expectedLeftRPM - leftSideRPM);
+    double rightSlip = std::abs(expectedRightRPM - rightSideRPM);
+
+    bool leftSlipping = leftSlip > SLIP_THRESHOLD;
+    bool rightSlipping = rightSlip > SLIP_THRESHOLD;
+
+    // Apply traction control corrections
+    if (leftSlipping || rightSlipping) {
+        // Reduce power to both sides
+        forwardVolts *= TRACTION_REDUCTION;
+        turnVolts *= TRACTION_REDUCTION;
+
+        if (debugOutput) {
+            printf("Traction Control: Left slip=%.1f, Right slip=%.1f, Power reduced\n", 
+                   leftSlip, rightSlip);
+        }
+    }
+
+    // Optional: Use inertial sensor for additional heading correction during slip
+    if ((leftSlipping || rightSlipping) && InertialGyro.installed()) {
+        double currentHeading = InertialGyro.heading(vex::rotationUnits::deg);
+        if (headingInitialized) {
+            double headingError = targetHeading - currentHeading;
+
+            // Normalize heading error to [-180, 180]
+            while (headingError > 180) headingError -= 360;
+            while (headingError < -180) headingError += 360;
+
+            // Apply small heading correction during traction control
+            double headingCorrection = headingError * HEADING_KP * 0.5; // Reduced gain during slip
+            headingCorrection = std::max(-MAX_HEADING_CORRECTION/2, 
+                                       std::min(MAX_HEADING_CORRECTION/2, headingCorrection));
+
+            turnVolts += headingCorrection;
+        }
+    }
 }
 
 // Function to apply stability control
-void applyStabilityControl(const double &forwardVolts)
+void applyStabilityControl(double &forwardVolts, double &turnVolts)
 {
-    double leftRPM = LeftDriveSmart.velocity(vex::velocityUnits::rpm);
-    double rightRPM = RightDriveSmart.velocity(vex::velocityUnits::rpm);
-
-    double minSpeed = std::min(std::abs(leftRPM), std::abs(rightRPM));
-
-    LeftDriveSmart.spin(vex::directionType::fwd, minSpeed, vex::voltageUnits::volt);
-    RightDriveSmart.spin(vex::directionType::fwd, minSpeed, vex::voltageUnits::volt);
+    if (!InertialGyro.installed()) {
+        return; // Cannot provide stability control without inertial sensor
+    }
+
+    double currentHeading = InertialGyro.heading(vex::rotationUnits::deg);
+    double currentPitch = InertialGyro.pitch(vex::rotationUnits::deg);
+    double currentRoll = InertialGyro.roll(vex::rotationUnits::deg);
+    double yawRate = InertialGyro.gyroRate(vex::axisType::zaxis, vex::velocityUnits::dps);
+
+    // Initialize target heading when first called or when not moving
+    if (!headingInitialized || (std::abs(forwardVolts) < 0.5 && std::abs(turnVolts) < 0.5)) {
+        targetHeading = currentHeading;
+        headingInitialized = true;
+    }
+
+    // Anti-tip feature: reduce power if excessive tilt detected
+    bool tippingDetected = (std::abs(currentPitch) > ANTI_TIP_THRESHOLD || 
+                           std::abs(currentRoll) > ANTI_TIP_THRESHOLD);
+
+    if (tippingDetected) {
+        forwardVolts *= 0.3; // Dramatically reduce forward power
+        turnVolts *= 0.3;    // Reduce turn power
+
+        if (debugOutput) {
+            printf("Anti-tip activated: Pitch=%.1f°, Roll=%.1f°\n", currentPitch, currentRoll);
+        }
+        return; // Skip normal stability control during anti-tip
+    }
+
+    // When driving mostly straight (small turn input), apply heading correction
+    if (std::abs(turnVolts) < 2.0 && std::abs(forwardVolts) > 1.0) {
+        double headingError = targetHeading - currentHeading;
+
+        // Normalize heading error to [-180, 180]
+        while (headingError > 180) headingError -= 360;
+        while (headingError < -180) headingError += 360;
+
+        // Apply proportional correction
+        double headingCorrection = headingError * HEADING_KP;
+        headingCorrection = std::max(-MAX_HEADING_CORRECTION, 
+                                   std::min(MAX_HEADING_CORRECTION, headingCorrection));
+
+        turnVolts += headingCorrection;
+
+        if (debugOutput && std::abs(headingCorrection) > 0.1) {
+            printf("Stability Control: Heading error=%.1f°, Correction=%.2fV\n", 
+                   headingError, headingCorrection);
+        }
+    }
+
+    // Limit turn rate to avoid instability (yaw rate limiting)
+    const double MAX_YAW_RATE = 180.0; // degrees per second
+    if (std::abs(yawRate) > MAX_YAW_RATE) {
+        double yawRateReduction = MAX_YAW_RATE / std::abs(yawRate);
+        turnVolts *= yawRateReduction;
+
+        if (debugOutput) {
+            printf("Yaw rate limiting: %.1f dps, reduction factor=%.2f\n", yawRate, yawRateReduction);
+        }
+    }
+
+    // Update target heading when intentionally turning
+    if (std::abs(turnVolts) > 2.0) {
+        targetHeading = currentHeading; // Use current heading as new target when turning
+    }
 }
 
 // Function to apply ABS
-void applyABS(double &brakeVolts)
+void applyABS(double &forwardVolts, double &turnVolts)
 {
-    double frontLeftMotorRPM = frontLeftMotor.velocity(vex::velocityUnits::rpm);
-    double rearLeftMotorRPM = rearLeftMotor.velocity(vex::velocityUnits::rpm);
-    double frontRightMotorRPM = frontRightMotor.velocity(vex::velocityUnits::rpm);
-    double rearRightMotorRPM = rearRightMotor.velocity(vex::velocityUnits::rpm);
-
-    double minSpeed = std::min({std::abs(frontLeftMotorRPM), std::abs(rearLeftMotorRPM), std::abs(frontRightMotorRPM), std::abs(rearRightMotorRPM)});
-
-    brakeVolts = minSpeed;
+    // Get current motor velocities
+    double frontLeftRPM = frontLeftMotor.velocity(vex::velocityUnits::rpm);
+    double rearLeftRPM = rearLeftMotor.velocity(vex::velocityUnits::rpm);
+    double frontRightRPM = frontRightMotor.velocity(vex::velocityUnits::rpm);
+    double rearRightRPM = rearRightMotor.velocity(vex::velocityUnits::rpm);
+
+    double currentVelocities[4] = {frontLeftRPM, rearLeftRPM, frontRightRPM, rearRightRPM};
+
+    // Detect braking condition: check if we're commanding low/negative power but still have momentum
+    bool brakingDetected = false;
+    double avgCurrentVelocity = (std::abs(frontLeftRPM) + std::abs(rearLeftRPM) + 
+                                std::abs(frontRightRPM) + std::abs(rearRightRPM)) / 4.0;
+
+    // Braking detected if commanding low power but robot still has significant velocity
+    if (std::abs(forwardVolts) < 2.0 && avgCurrentVelocity > 30.0) {
+        brakingDetected = true;
+    }
+
+    // Also detect if motors are actively being commanded to stop/reverse
+    if (forwardVolts < -1.0) { // Reverse/braking command
+        brakingDetected = true;
+    }
+
+    if (brakingDetected) {
+        bool lockupDetected = false;
+
+        // Check for sudden velocity drops indicating wheel lockup
+        for (int i = 0; i < 4; i++) {
+            double velocityDrop = std::abs(previousVelocities[i]) - std::abs(currentVelocities[i]);
+
+            if (velocityDrop > ABS_VELOCITY_THRESHOLD) {
+                lockupDetected = true;
+
+                if (debugOutput) {
+                    const char* motorNames[] = {"FL", "RL", "FR", "RR"};
+                    printf("ABS: %s wheel lockup detected, velocity drop=%.1f RPM\n", 
+                           motorNames[i], velocityDrop);
+                }
+                break;
+            }
+        }
+
+        // Apply ABS correction if lockup detected
+        if (lockupDetected) {
+            // Reduce braking force to prevent lockup
+            if (forwardVolts < 0) {
+                forwardVolts *= ABS_REDUCTION; // Reduce reverse/braking power
+            } else {
+                forwardVolts = std::max(forwardVolts, 1.0); // Maintain minimum forward power
+            }
+
+            // Also reduce turn power during ABS activation
+            turnVolts *= ABS_REDUCTION;
+
+            if (debugOutput) {
+                printf("ABS activated: Braking power reduced to %.2fV\n", forwardVolts);
+            }
+        }
+    }
+
+    // Store current velocities for next iteration
+    for (int i = 0; i < 4; i++) {
+        previousVelocities[i] = currentVelocities[i];
+    }
 }
 
 // Function to display drive mode menu
@@ -138,6 +342,57 @@ void userControl()
             }
         }
 
+        // Toggle debug output with Down button
+        static bool debugButtonPressed = false;
+        if (primaryController.ButtonDown.pressing() && !debugButtonPressed)
+        {
+            toggleDebugOutput();
+            debugButtonPressed = true;
+        }
+        else if (!primaryController.ButtonDown.pressing())
+        {
+            debugButtonPressed = false;
+        }
+
+        // Toggle control systems with shoulder buttons
+        static bool l1ButtonPressed = false, l2ButtonPressed = false, r1ButtonPressed = false;
+
+        // L1: Toggle Traction Control
+        if (primaryController.ButtonL1.pressing() && !l1ButtonPressed)
+        {
+            tractionControlEnabled = !tractionControlEnabled;
+            printf("Traction Control: %s\n", tractionControlEnabled ? "ENABLED" : "DISABLED");
+            l1ButtonPressed = true;
+        }
+        else if (!primaryController.ButtonL1.pressing())
+        {
+            l1ButtonPressed = false;
+        }
+
+        // L2: Toggle Stability Control
+        if (primaryController.ButtonL2.pressing() && !l2ButtonPressed)
+        {
+            stabilityControlEnabled = !stabilityControlEnabled;
+            printf("Stability Control: %s\n", stabilityControlEnabled ? "ENABLED" : "DISABLED");
+            l2ButtonPressed = true;
+        }
+        else if (!primaryController.ButtonL2.pressing())
+        {
+            l2ButtonPressed = false;
+        }
+
+        // R1: Toggle ABS
+        if (primaryController.ButtonR1.pressing() && !r1ButtonPressed)
+        {
+            absEnabled = !absEnabled;
+            printf("ABS: %s\n", absEnabled ? "ENABLED" : "DISABLED");
+            r1ButtonPressed = true;
+        }
+        else if (!primaryController.ButtonR1.pressing())
+        {
+            r1ButtonPressed = false;
+        }
+
         switch (currentDriveMode)
         {
         case configManager::DriveMode::LeftArcade:
@@ -206,19 +461,19 @@ void userControl()
             // Apply traction control if enabled
             if (tractionControlEnabled)
             {
-                applyTractionControl(forwardVolts);
+                applyTractionControl(forwardVolts, turnVolts);
             }
 
             // Apply stability control if enabled
             if (stabilityControlEnabled)
             {
-                applyStabilityControl(forwardVolts);
+                applyStabilityControl(forwardVolts, turnVolts);
             }
 
             // Apply ABS if enabled
             if (absEnabled)
             {
-                applyABS(forwardVolts);
+                applyABS(forwardVolts, turnVolts);
             }
 
             // Apply the calculated voltages to the motors