NIU Competition code final 20230930

src/globals.h

@@ -25,7 +25,8 @@ extern long last_p;
 #define max(x,y) ( (x) > (y) ? (x) : (y) )
 #define min(x,y) ( (x) < (y) ? (x) : (y) )
 
-#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
+#define MOTOR_MAX_POWER         127.0   // Highest value accepted by motor control functions
+#define DRIVE_MOTOR_MAX_POWER   64.0    // Maximum power for drive motors 
 
 // Drive modes
 #define DRIVE_STOP          0
@@ -55,22 +56,20 @@ extern long last_p;
 // Length of the buffer to monitor recent steering encoder positions to calculate speed
 // The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the steering motors' current speeds
 // This value must always be at least 2, otherwise the code will break due to there being an array with a zero or negative length or a division by zero
-#define ENCODER_BUFFER_ENTRY_COUNT 5
+#define ENCODER_BUFFER_ENTRY_COUNT 3
 
-
-// Number of encoder ticks per full rotation of each swerve drive steering motor
-#define STEERING_ENCODER_TICKS_PER_ROTATION     (1024.0 * 8.0)
-// Number of encoder ticks per degree of rotation for the swerve drive steering motors
-#define STEERING_ENCODER_TICKS_PER_DEGREE       (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0)
-
-// Maximum speed allowed for the steering motors (out of 127.0)
-#define STEERING_MOTOR_SPEED_LIMIT 15.0 // TODO as of 20230927, lower this if they're spinning too fast for the robot to handle
-
-// Start decelerating the steering motors linearly when they're within this many degrees of their target angle
-#define STEERING_SLOW_DELTA 5.0
-
-// Estimated acceleration delay of steering motors at low speeds (seconds)
-#define STEERING_ACCEL_SLOW_DELAY 0.20
+// Steering parameters
+#define STEERING_ENCODER_TICKS_PER_ROTATION     (1024.0 * 8.0)                                  // Number of encoder ticks per full rotation of each swerve drive steering motor
+#define STEERING_ENCODER_TICKS_PER_DEGREE       (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0)   // Number of encoder ticks per degree of rotation for the swerve drive steering motors
+#define STEERING_MOTOR_SPEED_LIMIT 80.0                                                         // Maximum speed allowed for the steering motors (out of 127.0)
+// Steering PID parameters
+#define STEERING_SLOW_DELTA 35.0                                                                // Start decelerating the steering motors linearly when they're within this many degrees of their target angle
+#define STEERING_ACCEL_SLOW_DELAY 0.20                                                          // Estimated acceleration delay of steering motors at low speeds (seconds)
+#define STEERING_TOLERANCE 1.0                                                                  // Steering tolerance in degrees
+#define STEERING_STALL_DETECT_ANGULAR_SPEED 5.0                                                 // Detect steering motor stall if measured angular speed is below this
+#define STEERING_SLOW_APPROACH_SPEED (0.16 * (MOTOR_MAX_POWER / STEERING_MOTOR_SPEED_LIMIT))    // Slow approach speed for steering motors
+#define STEERING_TOLERANCE_DISABLE_DRIVE 30.0                                                    // Disable the drive motors if any steering motor is off-target by more than this many degrees
+#define STEERING_HOVER_RANGE 10.0                                                               // Angular range where steering motors tend to hover around their targets
 
 // Claw status
 #define CLAW_UNKNOWN        1   // Position unknown

src/main.cpp

@@ -408,13 +408,11 @@ void set_motor(byte motor, int speed) {
   // 15 - 17 : arm servos
   // speed is -127 to 127
   
-  // TESTING DEBUG : commented out for testing
-  /*
   Serial.print("Driving motor ");
   Serial.print(motor);
   Serial.print(" with speed ");
   Serial.println(speed);
-  */
+  
 
   if (motor <= 4) {
     // swerve controls
@@ -481,22 +479,26 @@ void set_motor(byte motor, int speed) {
 
       stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
       stepperX.moveTo(stepperX_pos);
-      stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
-      stepperY.moveTo(stepperX_pos);
+      //stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+      //stepperY.moveTo(stepperX_pos);
       stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
       stepperZ.moveTo(stepperX_pos);
+
       stepperX.runState();
-      stepperY.runState();
+      //stepperY.runState();
       stepperZ.runState();
     } else {
       ioex1.digitalWrite(2, HIGH); // disable
-      stepperX_pos = stepperX.currentPosition();
+
+      // stepperX_pos = stepperX.currentPosition();
+
       stepperX.setCurrentPosition(stepperX_pos);
-      stepperY.setCurrentPosition(stepperX_pos);
+      //stepperY.setCurrentPosition(stepperX_pos);
       stepperZ.setCurrentPosition(stepperX_pos);
-      stepperX.stop();
-      stepperY.stop();
-      stepperZ.stop();
+
+      //stepperX.stop();
+      //stepperY.stop();
+      //stepperZ.stop();
       
       
     }
@@ -504,7 +506,7 @@ void set_motor(byte motor, int speed) {
   else if (motor == 15)
     arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
   else if (motor == 16)
-    arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+    arm2.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
   else if (motor == 17)
     arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
 }
@@ -983,11 +985,12 @@ void loop() {
   swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
   swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
 
+
   //DEBUG TESTING code:
+  Serial.printf("FL spin target %f \t\t at %f\r\n", swrv.front_left_target_spin, normalizeAngle(swrv.front_right_spin_angle));
   Serial.printf("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, normalizeAngle(swrv.front_right_spin_angle));
-  //Serial.printf("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, swrv.front_right_spin_angle);
-  //Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, swrv.back_left_spin_angle);
   Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, normalizeAngle(swrv.back_left_spin_angle));
+  Serial.printf("BR spin target %f \t\t at %f\r\n", swrv.back_right_target_spin, normalizeAngle(swrv.back_right_spin_angle));
   
   // Arm motor control (stepper motors), DPAD_UP to move arm up, DPAD_DOWN to move arm down, both or neither being pressed stops the arm
   float arm_speed = (float) (((int) getButton(DPAD_UP)) - ((int) getButton(DPAD_DOWN))); // TODO 20230929 confirm speed and polarity
@@ -995,7 +998,7 @@ void loop() {
 
   // Claw servo control
   int new_claw_command = CLAW_COMMAND_UNSET;
-  int claw_direction = getButton(CLAW_OPEN_BUTTON) - getButton(CLAW_CLOSE_BUTTON);
+  int claw_direction = ((int) getButton(CLAW_OPEN_BUTTON)) - ((int) getButton(CLAW_CLOSE_BUTTON));
   switch(claw_direction) {
     case 0:
       new_claw_command = CLAW_COMMAND_STAY;
@@ -1013,11 +1016,11 @@ void loop() {
   int new_tilt_command = TILT_COMMAND_UNSET;
   clawarm = updateTiltCommand(clawarm, new_tilt_command);
 
+
   telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude, previous_loop_processing_duration_core_0); // DEBUG ONLY, telemetry
 
+
   // update motors after calculation
-  // TESTING 20230929 comment out to test steering
-  
   set_motor(FLDRIVE, swrv.front_left_power);
   set_motor(BRDRIVE, swrv.back_right_power);
   set_motor(FRDRIVE, swrv.front_right_power);
@@ -1026,17 +1029,21 @@ void loop() {
 
   // Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
   spinlock_lock_core_0(&drive_power_command_spinlock_flag);
+
   power_data_transfer_fl = swrv.front_left_spin_power;
-  // TESTING 20230929 comment out since encoders not yet connected
   power_data_transfer_fr = swrv.front_right_spin_power;
   power_data_transfer_bl = swrv.back_left_spin_power;
   power_data_transfer_br = swrv.back_right_spin_power;
   spinlock_release(&drive_power_command_spinlock_flag);
 
   // update stepper motors
+  // TESTING: comment out this code to check performance impact
   set_motor(LIFTALL, clawarm.arm_set_motor_int);
 
   // update servos
+  Serial.printf("claw set motor int %i\r\n", clawarm.claw_set_motor_int);
+  set_motor(ARMSERVO1, clawarm.claw_set_motor_int);
+  set_motor(ARMSERVO2, - clawarm.claw_set_motor_int);
   /*
   // TODO: Figure out servo mapping
   set_motor(SERVOTILT, clawarm.tilt_set_motor_int);

src/swerve.cpp

@@ -54,24 +54,32 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
 swerve_drive updateSwerveCommand(swerve_drive input)
 {
     swerve_drive out = input;
-
+    float new_drive_coefficient = out.target_drive_power;
     // Set the new speed of the steering motors
-    if((out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) && out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
+    if(/*(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) &&*/ out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
         // Calculate the distance and direction each motor needs to steer from where it is now
         float front_left_delta = closestAngle(out.front_left_spin_angle, out.front_left_target_spin);
         float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_target_spin);
         float back_left_delta = closestAngle(out.back_left_spin_angle, out.back_left_target_spin);
         float back_right_delta = closestAngle(out.back_right_spin_angle, out.back_right_target_spin);
+        // Use the delta and speed of each steering motor to calculate the necessary speed
+        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta, out.front_left_measured_spin_speed);
+        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta, out.front_right_measured_spin_speed);
+        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_measured_spin_speed);
+        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta, out.back_right_measured_spin_speed);
 
-        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta);
-        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta);
-        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta);
-        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta);
-
+        float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta)));
+        if (max_abs_steering_delta > STEERING_TOLERANCE_DISABLE_DRIVE) {
+            new_drive_coefficient = 0;
+        }
+        Serial.printf("max_abs_steering_delta = %f\t\tndc = %f\r\n", max_abs_steering_delta, new_drive_coefficient);
 
         // TESTING DEBUG print 20230929
-        Serial.printf("FR delta = %f\t\tBL delta = %f\r\n", front_right_delta, back_left_delta);
-        Serial.printf("FR steer = %f\t\tBL steer = %f\r\n", out.front_right_spin_power, out.back_left_spin_power);
+        Serial.printf("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
+        Serial.printf("FR delta = %f\t\tFR steer = %f\r\n", front_right_delta, out.front_right_spin_power);
+        Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_spin_power);
+        Serial.printf("BR delta = %f\t\tBR steer = %f\r\n", back_right_delta, out.back_right_spin_power);
+        
 
     } else { // Stop the steering motors if the robot is stopped and not trying to move, or if steering is disabled
         out.front_left_spin_power = 0.0f;
@@ -81,25 +89,43 @@ swerve_drive updateSwerveCommand(swerve_drive input)
     }
 
     // Set the current drive power to the target drive power, TODO: this is TEMPORARY, add in something to slow the current (set) speed until the wheels are in the correct direction
-    out.current_drive_power = out.target_drive_power;  
-
+    //out.current_drive_power = out.target_drive_power;
     // Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
-    out.front_left_power = out.current_drive_power * out.front_left_coefficient * MOTOR_MAX_POWER;
-    out.front_right_power = out.current_drive_power * out.front_right_coefficient * MOTOR_MAX_POWER;
-    out.back_left_power = out.current_drive_power * out.back_left_coefficient * MOTOR_MAX_POWER;
-    out.back_right_power = out.current_drive_power * out.back_right_coefficient * MOTOR_MAX_POWER;
+    out.current_drive_power = new_drive_coefficient;
+    out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.back_left_power = new_drive_coefficient * out.back_left_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.back_right_power = new_drive_coefficient * out.back_right_coefficient * DRIVE_MOTOR_MAX_POWER;
     
     return out;
 }
 
-float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed of a steering motor based on its distance from its target angle
+float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed) // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
 {
-    float steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
     float abs_steering_delta = fabs(steering_delta);
-    if(abs_steering_delta > STEERING_SLOW_DELTA) { // In full speed range, still far enough away from the target angle
-        return steering_limit_signed;
+    if(abs_steering_delta > STEERING_SLOW_DELTA && abs_steering_delta > STEERING_TOLERANCE) { // In full speed range, still far enough away from the target angle
+        return STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
     } else { // Slow down the speed of the steering motor since it's close to its target angle
-        return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
+
+        float calc_steering_delta = steering_delta + (STEERING_ACCEL_SLOW_DELAY * current_angular_speed); // Modify the steering delta to the estimated delta in STEERING_ACCEL_SLOW_DELAY seconds to account for motor acceleration
+        float calc_steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (calc_steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above
+        float calc_abs_steering_delta = fabs(calc_steering_delta); // Update abs_steering_delta with the new steering_delta
+        float steering_speed_fraction = powf(calc_abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used
+        //return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
+        if(current_angular_speed < STEERING_STALL_DETECT_ANGULAR_SPEED || steering_speed_fraction < STEERING_SLOW_APPROACH_SPEED) { // Detect motor stall during approach and increase speed to allow for approach
+            steering_speed_fraction = STEERING_SLOW_APPROACH_SPEED;
+            
+            if(calc_abs_steering_delta < STEERING_HOVER_RANGE) { // Decrease speed further if the steering is extremely close to the target
+                steering_speed_fraction *= (calc_abs_steering_delta / STEERING_HOVER_RANGE);
+            } else if(abs_steering_delta < STEERING_HOVER_RANGE) {
+                steering_speed_fraction *= (abs_steering_delta / STEERING_HOVER_RANGE);
+            }
+        }
+        if(calc_abs_steering_delta < STEERING_TOLERANCE) { // Stop the steering motors if they are within the tolerance range
+            return 0.0f;
+        }
+
+        return calc_steering_limit_signed * steering_speed_fraction; // Apply the direction
     }
 }
 
@@ -242,7 +268,7 @@ swerve_drive rotationDrive(swerve_drive input, float target_speed) // Implementa
 
     //float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
 
-    out = setTargetSpin(out, 45.0, 135.0, 225.0, 315.0); // Set the target angle for each rotation motor
+    out = setTargetSpin(out, 45.0, 135.0, 315.0, 225.0); // Set the target angle for each rotation motor
     out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
     out = setDriveTargetPower(out, target_speed); // Set the power
 

src/swerve.h

@@ -81,7 +81,7 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
 
 swerve_drive updateSwerveCommand(swerve_drive input); // This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
 
-float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle
+float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed); // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
 
 swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Process new encoder data, calculate the speed and angle of the steering motors