adeck/rib-test-code
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

NIU Competition code final 20230930

Browse Source
This commit is contained in:
evlryah
2023-09-30 14:51:09 -05:00
parent 0a49ecfa40
commit 225af7fead
3 changed files with 52 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/globals.h
View File

@@ -25,7 +25,8 @@ extern long last_p;
#define max(x,y) ( (x) > (y) ? (x) : (y) ) #define max(x,y) ( (x) > (y) ? (x) : (y) )
#define min(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 // Drive modes
#define DRIVE_STOP 0 #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 // 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 // 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 // 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 // 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_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_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 // 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_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_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_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_SLOW_APPROACH_SPEED (0.16 * (MOTOR_MAX_POWER / STEERING_MOTOR_SPEED_LIMIT)) // 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_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 // Claw status
#define CLAW_UNKNOWN 1 // Position unknown #define CLAW_UNKNOWN 1 // Position unknown

56
src/main.cpp
View File

@@ -408,13 +408,11 @@ void set_motor(byte motor, int speed) {
// 15 - 17 : arm servos // 15 - 17 : arm servos
// speed is -127 to 127 // speed is -127 to 127
// TESTING DEBUG : commented out for testing
/*
Serial.print("Driving motor "); Serial.print("Driving motor ");
Serial.print(motor); Serial.print(motor);
Serial.print(" with speed "); Serial.print(" with speed ");
Serial.println(speed); Serial.println(speed);
*/
if (motor <= 4) { if (motor <= 4) {
// swerve controls // swerve controls
@@ -476,45 +474,27 @@ void set_motor(byte motor, int speed) {
} }
else if (motor == 14) { // all steppers together else if (motor == 14) { // all steppers together
if (abs(speed) > 0) { if (abs(speed) > 0) {
Serial.printf("DEBUG 1F1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
ioex1.digitalWrite(2, LOW); // enable ioex1.digitalWrite(2, LOW); // enable
stepperX_pos = speed * 96 + stepperX.currentPosition(); 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.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperX.moveTo(stepperX_pos); stepperX.moveTo(stepperX_pos);
stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); //stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperY.moveTo(stepperX_pos); //stepperY.moveTo(stepperX_pos);
//stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
//stepperZ.moveTo(stepperX_pos); stepperZ.moveTo(stepperX_pos);
Serial.printf("DEBUG 1F1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
stepperX.runState(); stepperX.runState();
stepperY.runState(); //stepperY.runState();
//stepperZ.runState(); stepperZ.runState();
Serial.printf("DEBUG 1F1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
} else { } else {
Serial.printf("DEBUG 1F2A: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
ioex1.digitalWrite(2, HIGH); // disable ioex1.digitalWrite(2, HIGH); // disable
Serial.printf("DEBUG 1F2B: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
// stepperX_pos = stepperX.currentPosition(); // stepperX_pos = stepperX.currentPosition();
Serial.printf("DEBUG 1F2C: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
stepperX.setCurrentPosition(stepperX_pos); stepperX.setCurrentPosition(stepperX_pos);
stepperY.setCurrentPosition(stepperX_pos); //stepperY.setCurrentPosition(stepperX_pos);
//stepperZ.setCurrentPosition(stepperX_pos); stepperZ.setCurrentPosition(stepperX_pos);
Serial.printf("DEBUG 1F2D: %llu ms\r\n", millis() - previous_loop_start_time_core_0);
//stepperX.stop(); //stepperX.stop();
//stepperY.stop(); //stepperY.stop();
@@ -526,7 +506,7 @@ void set_motor(byte motor, int speed) {
else if (motor == 15) else if (motor == 15)
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor == 16) 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) else if (motor == 17)
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); 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 = 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 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: //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("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 // Claw servo control
int new_claw_command = CLAW_COMMAND_UNSET; 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) { switch(claw_direction) {
case 0: case 0:
new_claw_command = CLAW_COMMAND_STAY; new_claw_command = CLAW_COMMAND_STAY;
@@ -1037,11 +1016,9 @@ void loop() {
int new_tilt_command = TILT_COMMAND_UNSET; int new_tilt_command = TILT_COMMAND_UNSET;
clawarm = updateTiltCommand(clawarm, new_tilt_command); 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 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 // update motors after calculation
set_motor(FLDRIVE, swrv.front_left_power); set_motor(FLDRIVE, swrv.front_left_power);
@@ -1050,28 +1027,23 @@ void loop() {
set_motor(BLDRIVE, swrv.back_left_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 // 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); 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_fl = swrv.front_left_spin_power;
power_data_transfer_fr = swrv.front_right_spin_power; power_data_transfer_fr = swrv.front_right_spin_power;
power_data_transfer_bl = swrv.back_left_spin_power; power_data_transfer_bl = swrv.back_left_spin_power;
power_data_transfer_br = swrv.back_right_spin_power; power_data_transfer_br = swrv.back_right_spin_power;
spinlock_release(&drive_power_command_spinlock_flag); 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 // update stepper motors
// TESTING: comment out this code to check performance impact // TESTING: comment out this code to check performance impact
set_motor(LIFTALL, clawarm.arm_set_motor_int); 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 // 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 // TODO: Figure out servo mapping
set_motor(SERVOTILT, clawarm.tilt_set_motor_int); set_motor(SERVOTILT, clawarm.tilt_set_motor_int);

46
src/swerve.cpp
View File

@@ -54,7 +54,7 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
swerve_drive updateSwerveCommand(swerve_drive input) swerve_drive updateSwerveCommand(swerve_drive input)
{ {
swerve_drive out = input; swerve_drive out = input;
float new_drive_coefficient = out.target_drive_power;
// Set the new speed of the steering motors // 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 // Calculate the distance and direction each motor needs to steer from where it is now
@@ -68,6 +68,11 @@ swerve_drive updateSwerveCommand(swerve_drive input)
out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_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.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 // 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("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
@@ -75,6 +80,7 @@ swerve_drive updateSwerveCommand(swerve_drive input)
Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_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); 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 } 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_left_spin_power = 0.0f;
out.front_right_spin_power = 0.0f; out.front_right_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 // 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 // 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.current_drive_power = new_drive_coefficient;
out.front_right_power = out.current_drive_power * out.front_right_coefficient * steering_disable_drive * MOTOR_MAX_POWER; out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
out.back_left_power = out.current_drive_power * out.back_left_coefficient * steering_disable_drive * MOTOR_MAX_POWER; out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
out.back_right_power = out.current_drive_power * out.back_right_coefficient * steering_disable_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; 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 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); 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); 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 } 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 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 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 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
abs_steering_delta = fabs(steering_delta); // Update abs_steering_delta with the new steering_delta float calc_abs_steering_delta = fabs(calc_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 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)); //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 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; // Commented out until this is necessary 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
} }
} }