diff --git a/src/globals.h b/src/globals.h index b3a27da..2ecbc2f 100644 --- a/src/globals.h +++ b/src/globals.h @@ -57,20 +57,18 @@ extern long last_p; // 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 +// 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 60.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 0.5 // 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.12 // Slow approach speed for steering motors +#define STEERING_TOLERANCE_DISABLE_DRIVE 2.0 // Disable the drive motors if any steering motor is off-target by more than this many degrees -// 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 // Claw status #define CLAW_UNKNOWN 1 // Position unknown diff --git a/src/main.cpp b/src/main.cpp index 5245d6c..41f2731 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -476,27 +476,49 @@ void set_motor(byte motor, int speed) { } else if (motor == 14) { // all steppers together if (abs(speed) > 0) { + + Serial.printf("DEBUG 1F1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + ioex1.digitalWrite(2, LOW); // enable stepperX_pos = speed * 96 + stepperX.currentPosition(); + Serial.printf("DEBUG 1F1B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperX.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); + //stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); + //stepperZ.moveTo(stepperX_pos); + + Serial.printf("DEBUG 1F1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + stepperX.runState(); stepperY.runState(); - stepperZ.runState(); + //stepperZ.runState(); + + Serial.printf("DEBUG 1F1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + } else { + Serial.printf("DEBUG 1F2A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + ioex1.digitalWrite(2, HIGH); // disable - stepperX_pos = stepperX.currentPosition(); + + Serial.printf("DEBUG 1F2B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + + // stepperX_pos = stepperX.currentPosition(); + + Serial.printf("DEBUG 1F2C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + stepperX.setCurrentPosition(stepperX_pos); stepperY.setCurrentPosition(stepperX_pos); - stepperZ.setCurrentPosition(stepperX_pos); - stepperX.stop(); - stepperY.stop(); - stepperZ.stop(); + //stepperZ.setCurrentPosition(stepperX_pos); + + Serial.printf("DEBUG 1F2D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + + //stepperX.stop(); + //stepperY.stop(); + //stepperZ.stop(); } @@ -983,11 +1005,13 @@ 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 + Serial.printf("DEBUG 1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + //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 @@ -1013,29 +1037,40 @@ void loop() { int new_tilt_command = TILT_COMMAND_UNSET; clawarm = updateTiltCommand(clawarm, new_tilt_command); + Serial.printf("DEBUG 1B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + 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 + Serial.printf("DEBUG 1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // 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); set_motor(BLDRIVE, swrv.back_left_power); + Serial.printf("DEBUG 1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // 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); + + Serial.printf("DEBUG 1E: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + 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); + Serial.printf("DEBUG 1F: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // update stepper motors + // TESTING: comment out this code to check performance impact set_motor(LIFTALL, clawarm.arm_set_motor_int); + Serial.printf("DEBUG 1G: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // update servos /* // TODO: Figure out servo mapping diff --git a/src/swerve.cpp b/src/swerve.cpp index 1d2b2e8..8dc8511 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -56,22 +56,24 @@ swerve_drive updateSwerveCommand(swerve_drive input) swerve_drive out = input; // 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); - - 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); + // 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); // 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 +83,35 @@ 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; + float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta))); + float steering_disable_drive = (float) (max_abs_steering_delta <= STEERING_TOLERANCE_DISABLE_DRIVE); // 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.front_left_power = out.current_drive_power * out.front_left_coefficient * steering_disable_drive * MOTOR_MAX_POWER; + out.front_right_power = out.current_drive_power * out.front_right_coefficient * steering_disable_drive * MOTOR_MAX_POWER; + out.back_left_power = out.current_drive_power * out.back_left_coefficient * steering_disable_drive * MOTOR_MAX_POWER; + out.back_right_power = out.current_drive_power * out.back_right_coefficient * steering_disable_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; + 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)); + + 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 steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above + abs_steering_delta = fabs(steering_delta); // Update abs_steering_delta with the new steering_delta + float steering_speed_fraction = powf(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) { // Detect motor stall during approach and increase speed to allow for approach + // steering_speed_fraction += STEERING_SLOW_APPROACH_SPEED; // Commented out until this is necessary + } + return steering_limit_signed * steering_speed_fraction; // Apply the direction } } @@ -242,7 +254,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 diff --git a/src/swerve.h b/src/swerve.h index 9855b29..4818378 100644 --- a/src/swerve.h +++ b/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