diff --git a/src/globals.h b/src/globals.h index b28cd72..aaf6067 100644 --- a/src/globals.h +++ b/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,8 +56,9 @@ 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 +<<<<<<< HEAD // Number of encoder ticks per full rotation of each swerve drive steering motor #define STEERING_ENCODER_TICKS_PER_ROTATION (1024.0 * 8.0) @@ -68,6 +70,20 @@ extern long last_p; // Start decelerating the steering motors linearly when they're within this many degrees of their target angle #define STEERING_SLOW_DELTA 30.0 +======= +// 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 +>>>>>>> modulus-plus-dev // Claw status #define CLAW_UNKNOWN 1 // Position unknown diff --git a/src/main.cpp b/src/main.cpp index 53ede92..e11fc9d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -44,8 +44,13 @@ PCF8574 ioex2(0x21, 20, 21); // JANK: soldered to pico or headers PioEncoder enc1(18); // Front Left +<<<<<<< HEAD PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929 PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929 +======= +PioEncoder enc2(0); // Front Right +PioEncoder enc3(2); // Back Left +>>>>>>> modulus-plus-dev PioEncoder enc4(14); // Back Right @@ -103,8 +108,8 @@ Sabertooth actuators(130, Serial2); #define TALON_PIN_3 7 #define TALON_PIN_4 9 // pins for arm servos -#define ARM_SERVO_PIN_1 2 -#define ARM_SERVO_PIN_2 3 +#define ARM_SERVO_PIN_1 26 +#define ARM_SERVO_PIN_2 27 #define ARM_SERVO_PIN_3 8 static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3; @@ -407,10 +412,13 @@ void set_motor(byte motor, int speed) { // 14 : drive 11-13 with identical position & speed // 15 - 17 : arm servos // speed is -127 to 127 + Serial.print("Driving motor "); Serial.print(motor); Serial.print(" with speed "); Serial.println(speed); + + if (motor <= 4) { // swerve controls speed *= (((motor % 2) * 2) - 1); // Flip motors 2 and 4 @@ -434,7 +442,8 @@ void set_motor(byte motor, int speed) { //stepperX.setSpeed((float)speed); if (abs(speed) > 0) ioex1.digitalWrite(2, LOW); // enable - + else + stepperX_pos = stepperX.currentPosition(); stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperX_pos = speed * 96 + stepperX.currentPosition(); @@ -446,7 +455,8 @@ void set_motor(byte motor, int speed) { //stepperY.setSpeed((float)speed); if (abs(speed) > 0) ioex1.digitalWrite(2, LOW); // enable - + else + stepperY_pos = stepperY.currentPosition(); stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperY_pos = speed * 96 + stepperY.currentPosition(); @@ -458,6 +468,8 @@ void set_motor(byte motor, int speed) { //stepperY.setSpeed((float)speed); if (abs(speed) > 0) ioex1.digitalWrite(2, LOW); // enable + else + stepperZ_pos = stepperZ.currentPosition(); stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperZ_pos = speed * 96 + stepperZ.currentPosition(); @@ -472,21 +484,34 @@ 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.setCurrentPosition(stepperX_pos); + //stepperY.setCurrentPosition(stepperX_pos); + stepperZ.setCurrentPosition(stepperX_pos); + + //stepperX.stop(); + //stepperY.stop(); + //stepperZ.stop(); + + } } 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)); } @@ -589,19 +614,11 @@ void setup() { ioex1.digitalWrite(3, HIGH); delay(2000); - digitalWrite(ALI1, LOW); - digitalWrite(BLI1, LOW); - digitalWrite(AHI1, LOW); - digitalWrite(BHI1, LOW); digitalWrite(ALI2, LOW); digitalWrite(BLI2, LOW); digitalWrite(AHI2, LOW); digitalWrite(BHI2, LOW); - pinMode(ALI1, OUTPUT); - pinMode(AHI1, OUTPUT); - pinMode(BLI1, OUTPUT); - pinMode(BHI1, OUTPUT); pinMode(ALI2, OUTPUT); pinMode(AHI2, OUTPUT); pinMode(BLI2, OUTPUT); @@ -694,7 +711,9 @@ void setup() { enc1.begin(); //enc1.flip(); enc2.begin(); + //enc2.flip(); enc3.begin(); + //enc3.flip(); enc4.begin(); Serial.println(" done"); delay(20); @@ -970,6 +989,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 + + + //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("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 @@ -977,7 +1003,11 @@ void loop() { // Claw servo control int new_claw_command = CLAW_COMMAND_UNSET; +<<<<<<< HEAD int claw_direction = getButton(CLAW_OPEN_BUTTON) - getButton(CLAW_CLOSE_BUTTON); +======= + int claw_direction = ((int) getButton(CLAW_OPEN_BUTTON)) - ((int) getButton(CLAW_CLOSE_BUTTON)); +>>>>>>> modulus-plus-dev switch(claw_direction) { case 0: new_claw_command = CLAW_COMMAND_STAY; @@ -995,13 +1025,29 @@ void loop() { int new_tilt_command = TILT_COMMAND_UNSET; clawarm = updateTiltCommand(clawarm, new_tilt_command); +<<<<<<< HEAD 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 +======= + + 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 + +>>>>>>> modulus-plus-dev // update motors after calculation 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); + + + // 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; + 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); // 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); @@ -1012,9 +1058,13 @@ void loop() { 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); @@ -1043,6 +1093,7 @@ void drive_control_core_1() { // Control drive motors from core 1 from loop1() f spinlock_release(&drive_power_command_spinlock_flag); // Release the spinlock // Set motors if the requested power is different than the previously requested power +<<<<<<< HEAD if(local_fl != power_data_transfer_prev_fl) { set_motor(FLSTEER, local_fl); } @@ -1055,6 +1106,20 @@ void drive_control_core_1() { // Control drive motors from core 1 from loop1() f if(local_br != power_data_transfer_prev_br) { set_motor(BRSTEER, local_br); } +======= + //if(local_fl != power_data_transfer_prev_fl) { + set_motor(FLSTEER, local_fl); + //} + //if(local_fr != power_data_transfer_prev_fr) { + set_motor(FRSTEER, local_fr); + //} + //if(local_bl != power_data_transfer_prev_bl) { + set_motor(BLSTEER, local_bl); + //} + //if(local_br != power_data_transfer_prev_br) { + set_motor(BRSTEER, local_br); + //} +>>>>>>> modulus-plus-dev // Set the previously requested power data to the current power data, will be read in the next loop power_data_transfer_prev_fl = local_fl; diff --git a/src/manipulator.cpp b/src/manipulator.cpp index 4b2be96..9e7e7b5 100644 --- a/src/manipulator.cpp +++ b/src/manipulator.cpp @@ -106,7 +106,7 @@ manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the a manipulator_arm out = input; arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed)); out.arm_speed = arm_speed; - out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed))); + out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed))) * 2; return out; } manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit) // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0 diff --git a/src/swerve.cpp b/src/swerve.cpp index 8d74cfb..b917e07 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -54,18 +54,33 @@ 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); - 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); + + 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("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; out.front_right_spin_power = 0.0f; @@ -74,24 +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 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_MOTOR_SPEED_LIMIT; + 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_MOTOR_SPEED_LIMIT * (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 } } @@ -234,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 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