From 225af7feadde55c20dc7ef1be234d4fdf68d8b75 Mon Sep 17 00:00:00 2001 From: evlryah Date: Sat, 30 Sep 2023 14:51:09 -0500 Subject: [PATCH] NIU Competition code final 20230930 --- src/globals.h | 15 +++++++------- src/main.cpp | 56 +++++++++++++------------------------------------- src/swerve.cpp | 46 ++++++++++++++++++++++++++--------------- 3 files changed, 52 insertions(+), 65 deletions(-) diff --git a/src/globals.h b/src/globals.h index 2ecbc2f..eeb0188 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,20 +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 // 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) +#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 0.5 // Steering tolerance in degrees +#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.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 - +#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 diff --git a/src/main.cpp b/src/main.cpp index 41f2731..bd31762 100644 --- a/src/main.cpp +++ b/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 @@ -476,45 +474,27 @@ 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); - - Serial.printf("DEBUG 1F1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + //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(); - //stepperZ.runState(); - - Serial.printf("DEBUG 1F1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - + //stepperY.runState(); + stepperZ.runState(); } else { - Serial.printf("DEBUG 1F2A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - ioex1.digitalWrite(2, HIGH); // disable - - 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); - - Serial.printf("DEBUG 1F2D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + //stepperY.setCurrentPosition(stepperX_pos); + stepperZ.setCurrentPosition(stepperX_pos); //stepperX.stop(); //stepperY.stop(); @@ -526,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)); } @@ -1005,7 +985,6 @@ 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)); @@ -1019,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; @@ -1037,11 +1016,9 @@ 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 set_motor(FLDRIVE, swrv.front_left_power); @@ -1050,28 +1027,23 @@ void loop() { 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; 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 + 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); diff --git a/src/swerve.cpp b/src/swerve.cpp index 8dc8511..b917e07 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -54,7 +54,7 @@ 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 // Calculate the distance and direction each motor needs to steer from where it is now @@ -68,12 +68,18 @@ swerve_drive updateSwerveCommand(swerve_drive input) 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; @@ -84,14 +90,12 @@ 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; - 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 * 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; + 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; } @@ -99,19 +103,29 @@ swerve_drive updateSwerveCommand(swerve_drive input) 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 + 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 - 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 + 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) { // 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 + 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); + } } - return steering_limit_signed * steering_speed_fraction; // Apply the direction + 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 } }