From 785cc6b5dbe6313df76c1416e11a8660b30b1e75 Mon Sep 17 00:00:00 2001 From: evlryah Date: Wed, 27 Sep 2023 21:35:00 -0500 Subject: [PATCH] Add steering angle tracking and dynamic loop delays --- src/globals.h | 5 +++++ src/main.cpp | 21 +++++++++++++++------ src/swerve.cpp | 9 ++++++++- src/swerve.h | 2 +- 4 files changed, 29 insertions(+), 8 deletions(-) diff --git a/src/globals.h b/src/globals.h index c6a2ef8..22af91b 100644 --- a/src/globals.h +++ b/src/globals.h @@ -14,9 +14,14 @@ extern long last_p; #define ANTICLOCKWISE 1 */ +// Loop timing +#define LOOP_DELAY_MS 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop + // Math things #define DEGREES_PER_RADIAN (180.0 / M_PI) #define RADIANS_PER_DEGREE (M_PI / 180.0) +#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 diff --git a/src/main.cpp b/src/main.cpp index 1c6e3be..2963a8c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -36,10 +36,11 @@ byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0 PCF8574 ioex1(0x20, 20, 21); PCF8574 ioex2(0x21, 20, 21); -PioEncoder enc1(18); // right1 -PioEncoder enc2(14); // left1 -PioEncoder enc3(26); // right2 -PioEncoder enc4(0); // left2 // JANK: soldered to pico or headers +// JANK: soldered to pico or headers +PioEncoder enc1(18); // Front Left +PioEncoder enc2(14); // Front Right +PioEncoder enc3(26); // Back Left +PioEncoder enc4(0); // Back Right // pins for arm servos #define ARM_SERVO_PIN_1 2 @@ -65,6 +66,8 @@ int stepperX_pos = 0; int stepperY_pos = 0; int stepperZ_pos = 0; +uint64_t previous_loop_start_time = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop + #define defMaxSpeed 8000 #define defAcceleration 8000 @@ -867,6 +870,8 @@ void print_encoder_positions() { // Print encoder positions for steering motors } void loop() { + previous_loop_start_time = millis(); // Track the start time of this loop to determine how long to sleep before the next loop + rp2040.wdt_reset(); comm_parse(); @@ -910,7 +915,7 @@ void loop() { swrv = rotationDrive(swrv, zeroed_rx); } - swrv = updateEncoderData(swrv, enc2.getCount(), enc1.getCount(), enc4.getCount(), enc3.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 print_encoder_positions(); // DEBUG ONLY, print steering motor encoder positions @@ -1071,7 +1076,11 @@ void loop() { } }*/ //delay(200); - delay(50); + + int64_t delay_time_ms = LOOP_DELAY_MS - (int64_t) (millis() - previous_loop_start_time); // Dynamically calculate delay time + if(delay_time_ms > 0) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS + delay(delay_time_ms); + } //DDYield(); } diff --git a/src/swerve.cpp b/src/swerve.cpp index 52da06a..32ab21e 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -91,7 +91,7 @@ float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed o } } -swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Load new encoder data into the swerve_drive struct, calculate the speed of the steering motors +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 { swerve_drive out = in; // Move values over 1 slot in the encoder data buffer to make room for new data, discard data at the end of the buffer @@ -132,6 +132,13 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron out.front_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1); out.back_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1); out.back_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1); + + // Calculate the current orientation of each drive wheel in degrees + out.front_left_spin_angle = ((float) (front_left_encoder - out.front_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE; + out.front_right_spin_angle = ((float) (front_right_encoder - out.front_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE; + out.back_left_spin_angle = ((float) (back_left_encoder - out.back_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE; + out.back_right_spin_angle = ((float) (back_right_encoder - out.back_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE; + return out; } diff --git a/src/swerve.h b/src/swerve.h index d2ede6a..f618eec 100644 --- a/src/swerve.h +++ b/src/swerve.h @@ -81,7 +81,7 @@ swerve_drive updateSwerveCommand(swerve_drive input); // This function calculate float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle -swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Load new encoder data into the swerve_drive struct, calculate the speed of the steering motors +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 swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode