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

Steering testing

Browse Source
This commit is contained in:
evlryah 2023-09-29 22:45:24 -05:00
parent 2e95e6afdc
commit e601e5a57c
9 changed files with 243 additions and 310 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/globals.h
View File

@ -15,12 +15,9 @@ extern long last_p;
*/ */
// Loop timing // Loop timing
// TODO TEMPORARY CHANGED TO 1000, NORMAL 50 #define LOOP_DELAY_MS_CORE_0 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
#define LOOP_DELAY_MS_CORE_0 500 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop #define LOOP_DELAY_MS_CORE_1 20 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
#define LOOP_DELAY_SECONDS_CORE_0 ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds #define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds
#define LOOP_DELAY_MS_CORE_1 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
#define LOOP_DELAY_SECONDS_CORE_1 ((float)LOOP_DELAY_MS_CORE_1 / 1000.0f) // Previous number expressed in seconds
// Math things // Math things
#define DEGREES_PER_RADIAN (180.0 / M_PI) #define DEGREES_PER_RADIAN (180.0 / M_PI)
@ -60,14 +57,20 @@ 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 // 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 5
// 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 // Number of encoder ticks per degree of rotation for the swerve drive steering motors
#define STEERING_ENCODER_TICKS_PER_DEGREE (1024.0 * 4.0) / 360.0 // TODO check as of 20230927 #define STEERING_ENCODER_TICKS_PER_DEGREE (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0)
// Maximum speed allowed for the steering motors (out of 127.0) // Maximum speed allowed for the steering motors (out of 127.0)
#define STEERING_MOTOR_SPEED_LIMIT 127.0 // TODO as of 20230927, lower this if they're spinning too fast for the robot to handle #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 // Start decelerating the steering motors linearly when they're within this many degrees of their target angle
#define STEERING_SLOW_DELTA 30.0 #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 // Claw status
#define CLAW_UNKNOWN 1 // Position unknown #define CLAW_UNKNOWN 1 // Position unknown
@ -81,6 +84,7 @@ extern long last_p;
// #define CLAW_COMMAND_STOP 1 // Stop immediately, no matter the location // #define CLAW_COMMAND_STOP 1 // Stop immediately, no matter the location
#define CLAW_COMMAND_CLOSE 2 // Close the claw #define CLAW_COMMAND_CLOSE 2 // Close the claw
#define CLAW_COMMAND_OPEN 3 // Open the claw #define CLAW_COMMAND_OPEN 3 // Open the claw
#define CLAW_COMMAND_STAY 4 // Maintain the current position
// Claw things // Claw things
#define CLAW_OPEN_ANGLE 90.0f // Open position of the claw #define CLAW_OPEN_ANGLE 90.0f // Open position of the claw
@ -88,18 +92,32 @@ extern long last_p;
#define CLAW_DEFAULT_ANGLE 0.0f // Default starting claw position #define CLAW_DEFAULT_ANGLE 0.0f // Default starting claw position
// Tilt servo control parameters // Tilt servo control parameters
#define TILT_DEGREES_PER_SECOND 50.0f // Speed in degrees per second that the tilt motor will move up or down (except when resetting to a flat position)
#define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update #define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update
#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS_CORE_0) // Previous value expressed as a number of control loops to wait between updates #define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates
#define TILT_ANGLE_UPDATE_DISTANCE 10.0f // Distance in degrees to shift the servo angle by each update #define TILT_ANGLE_UPDATE_DISTANCE (TILT_DEGREES_PER_SECOND * TILT_ANGLE_MIN_UPDATE_INTERVAL) // Distance in degrees to shift the servo angle by each update
#define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo #define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo
#define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo #define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo
#define TILT_FLAT_ANGLE 0.0f // Default/flat/starting angle for the tilt servo #define TILT_FLAT_ANGLE 0.0f // Default/flat/starting angle for the tilt servo
// Tilt servo commands // Tilt servo commands
#define TILT_COMMAND_UNSET 0 #define TILT_COMMAND_UNSET 0 // Command not yet set, go to default (flat) position
#define TILT_COMMAND_UP 1 #define TILT_COMMAND_RESET 1 // Reset the tilt servo to the flat position
#define TILT_COMMAND_DOWN 2 #define TILT_COMMAND_UP 2 // Raise the tilt servo
#define TILT_COMMAND_DOWN 3 // Lower the silt servo
// Control parameters, specify which buttons control new_angle actions
#define ARM_UP_BUTTON DPAD_UP
#define ARM_DOWN_BUTTON DPAD_DOWN
#define CLAW_OPEN_BUTTON DPAD_LEFT
#define CLAW_CLOSE_BUTTON DPAD_RIGHT
#define TILT_UP_BUTTON DIAMOND_UP
#define TILT_DOWN_BUTTON DIAMOND_DOWN
#define TILT_RESET_BUTTON DIAMOND_LEFT
// Button definitions
#define SerComm Serial1 //Serial port connected to Xbee #define SerComm Serial1 //Serial port connected to Xbee
#define DIAMOND_LEFT 0 #define DIAMOND_LEFT 0
#define DIAMOND_DOWN 1 #define DIAMOND_DOWN 1

373
src/main.cpp
View File

@ -15,6 +15,7 @@
#include <107-Arduino-Servo-RP2040.h> #include <107-Arduino-Servo-RP2040.h>
#include "swerve.h" #include "swerve.h"
#include "manipulator.h" #include "manipulator.h"
#include "spinlock.h"
const char* ssid = "TEST"; const char* ssid = "TEST";
const char* password = "pink4bubble"; const char* password = "pink4bubble";
@ -43,9 +44,9 @@ PCF8574 ioex2(0x21, 20, 21);
// JANK: soldered to pico or headers // JANK: soldered to pico or headers
PioEncoder enc1(18); // Front Left PioEncoder enc1(18); // Front Left
PioEncoder enc2(14); // Front Right PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929
PioEncoder enc3(27); // Back Left PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929
PioEncoder enc4(0); // Back Right PioEncoder enc4(14); // Back Right
@ -68,10 +69,25 @@ int stepperX_pos = 0;
int stepperY_pos = 0; int stepperY_pos = 0;
int stepperZ_pos = 0; int stepperZ_pos = 0;
// Loop management
uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
uint64_t previous_loop_processing_duration_core_0 = 0, previous_loop_processing_duration_core_1 = 0; // Processing time for the previous loop on each core uint64_t previous_loop_processing_duration_core_0 = 0, previous_loop_processing_duration_core_1 = 0; // Processing time for the previous loop on each core
uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop
// Motor power communication between cores
byte drive_power_command_spinlock_flag = SPINLOCK_UNSET; // Spinlock tracking flag
// Variables used to transfer steering motor power between cores, covered by spinlock
int power_data_transfer_fl = 0;
int power_data_transfer_fr = 0;
int power_data_transfer_bl = 0;
int power_data_transfer_br = 0;
// Variables used to track previous requested motor power, only used by core 1, sabertooth not called if motor power unchanged since previous loop
int power_data_transfer_prev_fl = 0;
int power_data_transfer_prev_fr = 0;
int power_data_transfer_prev_bl = 0;
int power_data_transfer_prev_br = 0;
#define defMaxSpeed 8000 #define defMaxSpeed 8000
#define defAcceleration 8000 #define defAcceleration 8000
@ -112,14 +128,14 @@ int right_cooldown = 0;
int olddisplay = 99999; // guarantee a change when first value comes in int olddisplay = 99999; // guarantee a change when first value comes in
// motor indeces for set_motor() function // motor indeces for set_motor() function
#define FLSTEER 1 #define FRSTEER 1
#define FRSTEER 2 #define FLSTEER 2
#define BLSTEER 3 #define BRSTEER 3
#define BRSTEER 4 #define BLSTEER 4
#define FLDRIVE 5 #define FLDRIVE 5
#define FRDRIVE 6 #define BRDRIVE 6
#define BLDRIVE 7 #define BLDRIVE 7
#define BRDRIVE 8 #define FRDRIVE 8
#define EXTEND1 9 #define EXTEND1 9
#define EXTEND2 10 #define EXTEND2 10
#define LIFT1 11 #define LIFT1 11
@ -384,19 +400,25 @@ void set_dec(byte num) {
} }
void set_motor(byte motor, int speed) { void set_motor(byte motor, int speed) {
// 1 - 4 : swivel motors on Sabertooth 1-2 // 1 - 4 : swivel motors on Sabertooth 1-2 (Clockwise: FR, FL, BR, BL)
// 5 - 8 : drive motors on Talon SRX // 5 - 8 : drive motors on Talon SRX (Forward: FL, BR, BL, FL)
// 9 - 10 : actuators on Sabertooth 3 // 9 - 10 : actuators on Sabertooth 3
// 11 - 13 : Steppers on slave board // 11 - 13 : Steppers on slave board
// 14 : drive 11-13 with identical position & speed // 14 : drive 11-13 with identical position & 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
speed *= (((motor % 2) * 2) - 1); // Flip motors 2 and 4
swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed); swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
} }
else if (motor == 5) else if (motor == 5)
@ -675,7 +697,7 @@ void setup() {
Serial.print("Initializing encoders.."); Serial.print("Initializing encoders..");
set_hex(0x5); set_hex(0x5);
enc1.begin(); enc1.begin();
enc1.flip(); //enc1.flip();
enc2.begin(); enc2.begin();
enc3.begin(); enc3.begin();
enc4.begin(); enc4.begin();
@ -859,7 +881,8 @@ void print_status() {
} }
void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_float, float zeroed_ry_float, int drive_mode, \ void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_float, float zeroed_ry_float, int drive_mode, \
float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude) { // Print encoder positions for steering motors float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude, \
uint64_t processing_time) { // Print encoder positions for steering motors
char buffer[300] = "\0"; char buffer[300] = "\0";
@ -868,10 +891,11 @@ void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_flo
//SerComm.println(buffer); //SerComm.println(buffer);
// Encoder data and loop number // Encoder data and loop number
sprintf(buffer, "Loop = %llu E1 = %i E2 = %i E3 = %i E4 = %i z_lx = %f z_ly = %f z_rx = %f z_ry = %f mode = %i mag-L = %f ang-L = %f mag-R = %f ang-R = %f", \ sprintf(buffer, "Loop = %llu E1 = %i E2 = %i E3 = %i E4 = %i z_lx = %f z_ly = %f z_rx = %f z_ry = %f mode = %i mag-L = %f ang-L = %f mag-R = %f ang-R = %f proctime = %llu", \
loop_counter_core_0, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \ loop_counter_core_0, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, drive_mode, \ zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, drive_mode, \
left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle); left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle, \
processing_time);
Serial.println(buffer); Serial.println(buffer);
SerComm.println(buffer); SerComm.println(buffer);
} }
@ -949,56 +973,61 @@ void loop() {
swrv.enable_steering = false; swrv.enable_steering = false;
break; break;
} }
swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct swrv = updateEncoderData(swrv, enc1.getCount(), enc3.getCount(), enc4.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
// Arm motor control (stepper motors) //DEBUG TESTING code:
float arm_speed = 0.0f; // TODO as of 20230928: PLACEHOLDER for input for arm speed Serial.printf("FL spin target %f \t\t at %f\r\n", swrv.front_left_target_spin, normalizeAngle(swrv.front_left_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("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
clawarm = setArmSpeed(clawarm, arm_speed); clawarm = setArmSpeed(clawarm, arm_speed);
// 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);
// TODO select action for claw switch(claw_direction) {
*/ case 0:
new_claw_command = CLAW_COMMAND_STAY;
break;
case 1:
new_claw_command = CLAW_COMMAND_OPEN;
break;
case -1:
new_claw_command = CLAW_COMMAND_CLOSE;
break;
}
clawarm = updateClawCommand(clawarm, new_claw_command); clawarm = updateClawCommand(clawarm, new_claw_command);
// Tilt servo control // Tilt servo control
int new_tilt_command = TILT_COMMAND_UNSET; int new_tilt_command = TILT_COMMAND_UNSET;
/*
// TODO select action for the tilt servo
*/
clawarm = updateTiltCommand(clawarm, new_tilt_command); clawarm = updateTiltCommand(clawarm, new_tilt_command);
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); // DEBUG ONLY, print steering motor encoder positions 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
// update motors after calculation // update motors after calculation
// TESTING 20230929 comment out to test steering
/* /*
set_motor(FLSTEER, swrv.front_left_spin_power);
set_motor(BRSTEER, swrv.back_right_spin_power);
set_motor(FRSTEER, swrv.front_right_spin_power);
set_motor(BLSTEER, swrv.back_left_spin_power);
set_motor(FLDRIVE, swrv.front_left_power); set_motor(FLDRIVE, swrv.front_left_power);
set_motor(BRDRIVE, swrv.back_right_power); set_motor(BRDRIVE, swrv.back_right_power);
set_motor(FRDRIVE, swrv.front_right_power); set_motor(FRDRIVE, swrv.front_right_power);
set_motor(BLDRIVE, swrv.back_left_power); set_motor(BLDRIVE, swrv.back_left_power);
*/ */
int loop_motor_change_interval = 8;
if(loop_counter_core_0 % loop_motor_change_interval == 0) { // Only change motors every 20 loops, or 2 seconds
int target_active_motor = ((loop_counter_core_0 / loop_motor_change_interval) % 4) + 5;
for(int i = 5; i <= 8; i++) { // Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
// 20 for drive motors, 120 for steering motors spinlock_lock_core_0(&drive_power_command_spinlock_flag);
set_motor(i, (20 + (100 * (i < 4))) * (i == target_active_motor)); 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;
Serial.print("Powering motor"); //power_data_transfer_bl = swrv.back_left_spin_power;
Serial.println(target_active_motor); power_data_transfer_br = swrv.back_right_spin_power;
} spinlock_release(&drive_power_command_spinlock_flag);
// update stepper motors // update stepper motors
// TESTING temporarily disabled for motor testing above on 20230929 set_motor(LIFTALL, clawarm.arm_set_motor_int);
//set_motor(LIFTALL, clawarm.arm_set_motor_int);
// update servos // update servos
/* /*
@ -1009,158 +1038,9 @@ void loop() {
*/ */
/////////////////////////////////////////////////////////////
// END OF MODULUS PLUS CODE UNTIL THE END OF THIS FUNCTION //
/////////////////////////////////////////////////////////////
// Goliath / 2 side arcade tank drive code below
/*int zeroed_power = -1 * ((int)(astate->stickX) - 127);
int zeroed_turn = ((int)(astate->stickY) - 127);
if (true) { //fb != NULL) {
//int x = fb->x - 127;
//int y = - fb->y + 127;
int x = zeroed_turn;
int y = zeroed_power;
//Serial.print(x);
//Serial.print(" ");
//Serial.println(y);
double rawdriveangle = atan2(x, y);
double driveangle = rawdriveangle * 180 / 3.1415926;
target_left_power = y;
target_right_power = y;
target_left_power += x;
target_right_power += -x;
target_left_power = constrain(target_left_power, -127, 127);
target_right_power = constrain(target_right_power, -127, 127);
if(turbo) {
target_left_power *= 2;
target_right_power *= 2;
}
target_left_power = target_left_power * 0.675;
target_right_power = target_right_power * 0.675;
}
if(turbo)
acceleration = 8;
else
acceleration = 3;
if(left_cooldown > 0)
left_cooldown --;
if(abs(target_left_power) <= 4 && abs(left_power) > 5) {
left_power = 0;
left_cooldown = 2;
}
else if(target_left_power >= left_power + acceleration && left_cooldown == 0)
left_power += acceleration;
else if(acceleration > target_left_power - left_power && left_cooldown == 0)
left_power = target_left_power;
else if(target_left_power <= left_power - acceleration && left_cooldown == 0)
left_power -= acceleration;
else if(acceleration > left_power - target_left_power && left_cooldown == 0)
left_power = target_left_power;
if(right_cooldown > 0)
right_cooldown --;
if(abs(target_right_power) <= 4 && abs(right_power) > 5) {
right_power = 0;
right_cooldown = 2;
}
else if(target_right_power >= right_power + acceleration && right_cooldown == 0)
right_power += acceleration;
else if(acceleration > target_right_power - right_power && right_cooldown == 0)
right_power = target_right_power;
else if(target_right_power <= right_power - acceleration && right_cooldown == 0)
right_power -= acceleration;
else if(acceleration > right_power - target_right_power && right_cooldown == 0)
right_power = target_right_power;
int avg_speed = (abs(right_power) + abs(left_power))/2;
//SerComm.println();
set_hex(avg_speed);
//drive_right(right_enabled, right_power);
//drive_left(left_enabled, -left_power);
SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap()));
*/
//if(left_power != target_left_power || right_power != target_right_power)
//delay(1000);
//set_digit(0, 6);
//set_digit(0, 10);
//set_digit(1, 9);
//set_digit(1, 10);
//set_digit(2, 8);
//set_digit(2, 10);
//set_digit(3, 8);
//set_digit(3, 10);
//set_digit(4, 8);
//set_digit(4, 10);
/*if (mode == 0) {
set_raw(count / 8, count % 8);
if (count < 39) {
count ++;
} else {
count = 0;
mode = 1;
delay(100);
}
}*/
//print_status();
//drive_right(right_enabled, 10);
//drive_left(left_enabled, 10);
/*if (millis() % 3000 > 1500) {
set_mosfet(0, LOW);
set_mosfet(1, LOW);
//ioex2.digitalWrite(7, LOW);
}
if (millis() % 3000 < 1500) {
set_mosfet(0, HIGH);
set_mosfet(1, HIGH);
//ioex2.digitalWrite(7, HIGH);
}*/
/*if (mode == 1) {
set_dec(count);
drive_right(right_enabled, count);
//set_hex(count);
if (count < 40) {
count += 5;
} else {
//count = 0;
mode = 2;
}
}
if (mode == 2) {
set_dec(count);
drive_right(right_enabled, count);
//set_hex(count);
if (count > 5) {
count -= 5;
} else {
//count = 0;
mode = 1;
}
}*/
//delay(200);
previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0; previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0;
int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time
if(delay_time_ms > 0/* && delay_time_ms < 100*/) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0 if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
delay(delay_time_ms); delay(delay_time_ms);
} }
loop_counter_core_0++; loop_counter_core_0++;
@ -1168,88 +1048,41 @@ void loop() {
} }
void drive_control_core_1() { // Control drive motors from core 1 from loop1() function
// Lock the steering motor power data to read it
spinlock_lock_core_1(&drive_power_command_spinlock_flag);
int local_fl = power_data_transfer_fl;
int local_fr = power_data_transfer_fr;
int local_bl = power_data_transfer_bl;
int local_br = power_data_transfer_br;
spinlock_release(&drive_power_command_spinlock_flag); // Release the spinlock
// Set motors if the requested power is different than the previously requested power
//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);
//}
// 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;
power_data_transfer_prev_fr = local_fr;
power_data_transfer_prev_bl = local_bl;
power_data_transfer_prev_br = local_br;
}
void loop1() { void loop1() {
previous_loop_start_time_core_1 = millis(); previous_loop_start_time_core_1 = millis();
rp2040.wdt_reset(); rp2040.wdt_reset();
//drive_left(left_enabled, 255);
//digitalWrite(LED_BUILTIN, HIGH);
if(loop_counter_core_1 == 20) {
//print_status();
loop_counter_core_1 = 0;
delay(25);
}
else {
delay(25);
//loop_counter_core_1++;
}
//SerComm.println("update");
//left_enabled = try_enable_left(left_enabled, get_voltage(1));
//right_enabled = try_enable_right(right_enabled, get_voltage(0));
//digitalWrite(LED_BUILTIN, LOW);
/*if (stepperX.distanceToGo() == 0) { // Give stepper something to do...
if (stepperXdir) {
Serial.println("Driving stepper");
stepperX.moveTo(stepsToGo);
} else {
Serial.println("Driving stepper");
stepperX.moveTo(-stepsToGo);
}
stepperX.runState();
stepperXdir = !stepperXdir;
}
if (stepperY.distanceToGo() == 0) { // Give stepper something to do...
if (stepperYdir)
stepperY.moveTo(stepsToGo);
else
stepperY.moveTo(-stepsToGo);
stepperY.runState();
stepperYdir = !stepperYdir;
}
if (stepperZ.distanceToGo() == 0) { // Give stepper something to do...
if (stepperZdir)
stepperZ.moveTo(stepsToGo);
else
stepperZ.moveTo(-stepsToGo);
stepperZ.runState();
stepperZdir = !stepperZdir;
}*/
if (mode == 1) {
//set_hex(count);
if (count < 125) {
count += 1;
} else {
//count = 0;
mode = 2;
}
}
if (mode == 2) {
//set_hex(count);
if (count > -125) {
count -= 1;
} else {
//count = 0;
mode = 1;
}
}
/*for (int x = 5; x < 9; x++) {
set_motor(x, count);
}
swivel[0].motor(0, 127);
swivel[0].motor(1, 127);
swivel[1].motor(0, 127);
swivel[1].motor(1, 127);
set_motor(14, count); // drive all steppers in sync
digitalWrite(0, HIGH);
digitalWrite(1, HIGH);
digitalWrite(2, HIGH);
digitalWrite(3, HIGH);*/
drive_control_core_1();
previous_loop_processing_duration_core_1 = millis() - previous_loop_start_time_core_1; previous_loop_processing_duration_core_1 = millis() - previous_loop_start_time_core_1;
int64_t delay_time_ms = LOOP_DELAY_MS_CORE_1 - (int64_t) previous_loop_processing_duration_core_1; // Dynamically calculate delay time int64_t delay_time_ms = LOOP_DELAY_MS_CORE_1 - (int64_t) previous_loop_processing_duration_core_1; // Dynamically calculate delay time

23
src/manipulator.cpp
View File

@ -39,22 +39,28 @@ manipulator_arm setTiltAngle(manipulator_arm input, float new_tilt_angle) // Int
manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor
{ {
manipulator_arm out = input; manipulator_arm out = input;
if(new_tilt_command != TILT_COMMAND_UNSET && out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Ignore value of 0, make sure that its been long enough since the last update if(new_tilt_command != out.tilt_command || out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Make sure that it's been long enough since the last update to not spam the servo command
float tilt_angle_offset_direction = 0.0f; float new_angle = out.tilt_target_angle; // Maintain the existing angle by default
switch(new_tilt_command) { switch(new_tilt_command) {
case TILT_COMMAND_UNSET:
new_angle = out.tilt_target_angle; // Maintain the existing angle
case TILT_COMMAND_RESET:
new_angle = TILT_FLAT_ANGLE;
case TILT_COMMAND_UP: case TILT_COMMAND_UP:
tilt_angle_offset_direction = 1.0f; new_angle = out.tilt_target_angle + TILT_ANGLE_UPDATE_DISTANCE;
break; break;
case TILT_COMMAND_DOWN: case TILT_COMMAND_DOWN:
tilt_angle_offset_direction = -1.0f; new_angle = out.tilt_target_angle - TILT_ANGLE_UPDATE_DISTANCE;
break; break;
} }
float angle_offset = TILT_ANGLE_UPDATE_DISTANCE * tilt_angle_offset_direction; // Degrees that the target angle is being changed by out = setTiltAngle(out, new_angle);
out = setTiltAngle(out, out.tilt_target_angle + angle_offset); out.tilt_angle_loops_since_update = 0; // Reset the counter tracking loops since the last update, since an update was just performed
} else { // Increment the number of loops since the previous update, since an update was not performed during this loop } else { // Increment the number of loops since the previous update, since an update was not performed during this loop
out.tilt_angle_loops_since_update++; out.tilt_angle_loops_since_update++;
} }
out.tilt_command = new_tilt_command; // Save the new command to check whether it has changed in the next loop
return out; return out;
} }
@ -81,6 +87,9 @@ manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) /
case CLAW_COMMAND_CLOSE: case CLAW_COMMAND_CLOSE:
new_claw_angle = CLAW_CLOSED_ANGLE; new_claw_angle = CLAW_CLOSED_ANGLE;
break; break;
case CLAW_COMMAND_STAY:
new_claw_angle = out.claw_position;
break;
default: default:
new_claw_angle = CLAW_DEFAULT_ANGLE; new_claw_angle = CLAW_DEFAULT_ANGLE;
} }
@ -92,7 +101,7 @@ manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) /
// Arm functions (stepper motors) // Arm functions (stepper motors)
manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the arm's speed manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the arm's speed, must be between -1.0 and 1.0
{ {
manipulator_arm out = input; manipulator_arm out = input;
arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed)); arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed));

2
src/manipulator.h
View File

@ -51,7 +51,7 @@ manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command);
// Arm functions (stepper motors) // Arm functions (stepper motors)
manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed, must be between -1.0 and 1.0
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 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

34
src/spinlock.cpp Normal file
View File

@ -0,0 +1,34 @@
#include <Arduino.h>
#include "globals.h"
#include "spinlock.h"
void spinlock_lock_core_0(byte* spinlock_flag) // Lock a spinlock from core 0
{
while(*spinlock_flag == SPINLOCK_LOCK_CORE_1) {} // Wait until the data is not locked by the other core
*spinlock_flag = SPINLOCK_LOCK_CORE_0; // Try setting the flag to lock the data
// Wait to see if the memory was overwritten by the other core
delay(1);
if(*spinlock_flag == SPINLOCK_LOCK_CORE_1 || *spinlock_flag == SPINLOCK_OPEN) { // The other core locked the data (and currently holds a lock or has released it), recurse to try again
spinlock_lock_core_0(spinlock_flag);
return;
}
return;
}
void spinlock_lock_core_1(byte* spinlock_flag) // Lock a spinlock from core 1
{
while(*spinlock_flag == SPINLOCK_LOCK_CORE_1) {} // Wait until the data is not locked by the other core
*spinlock_flag = SPINLOCK_LOCK_CORE_0; // Try setting the flag to lock the data
// Wait to see if the memory was overwritten by the other core
delay(1);
if(*spinlock_flag == SPINLOCK_LOCK_CORE_1 || *spinlock_flag == SPINLOCK_OPEN) { // The other core locked the data (and currently holds a lock or has released it), recurse to try again
spinlock_lock_core_0(spinlock_flag);
return;
}
return;
}
void spinlock_release(byte* spinlock_flag) // Release a spinlock
{
*spinlock_flag = SPINLOCK_OPEN;
}

14
src/spinlock.h Normal file
View File

@ -0,0 +1,14 @@
#include "globals.h"
// Spinlock flags
#define SPINLOCK_UNSET 0
#define SPINLOCK_OPEN 1
#define SPINLOCK_LOCK_CORE_0 2
#define SPINLOCK_LOCK_CORE_1 3
void spinlock_lock_core_0(byte* spinlock_flag); // Lock a spinlock from core 0
void spinlock_lock_core_1(byte* spinlock_flag); // Lock a spinlock from core 1
void spinlock_release(byte* spinlock_flag); // Release a spinlock

43
src/swerve.cpp
View File

@ -62,10 +62,17 @@ swerve_drive updateSwerveCommand(swerve_drive input)
float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_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_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); 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_left_spin_power = calculateSteeringMotorSpeed(front_left_delta);
out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta); out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta);
out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta); out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta);
out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta); out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta);
// TESTING DEBUG print 20230929
Serial.printf("FL delta = %f\t\tBR delta = %f\r\n", front_left_delta, back_right_delta);
Serial.printf("FL steer = %f\t\tBR steer = %f\r\n", out.front_left_spin_power, 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;
@ -87,11 +94,12 @@ swerve_drive updateSwerveCommand(swerve_drive input)
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) // Calculate the speed of a steering motor based on its distance from its target angle
{ {
float steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
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) { // In full speed range, still far enough away from the target angle
return STEERING_MOTOR_SPEED_LIMIT; return steering_limit_signed;
} 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
return STEERING_MOTOR_SPEED_LIMIT * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA)); return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
} }
} }
@ -146,15 +154,27 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
return out; return out;
} }
float normalizeAngle(float angle) { // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees int normalizeEncoderData(int encoder_data) // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
{
encoder_data %= (int) STEERING_ENCODER_TICKS_PER_ROTATION;
encoder_data += (STEERING_ENCODER_TICKS_PER_ROTATION * (encoder_data < 0));
return encoder_data;
}
float relativeAngleFromEncoder(int encoder_data_relative ) // Calculate the relative angle from the difference between 2 encoder data points
{
return ((float) normalizeEncoderData(encoder_data_relative)) / ((float) STEERING_ENCODER_TICKS_PER_DEGREE);
}
float normalizeAngle(float angle) // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
{
angle = fmod(angle, 360); angle = fmod(angle, 360);
if(angle < 0.0) { angle += (360 * (angle < 0.0));
angle += 360;
}
return angle; return angle;
} }
swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right) { // Set the motor speed coefficients for each motor swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right) // Set the motor speed coefficients for each motor
{
swerve_drive out = input; swerve_drive out = input;
out.front_left_coefficient = front_left; out.front_left_coefficient = front_left;
out.front_right_coefficient = front_right; out.front_right_coefficient = front_right;
@ -163,7 +183,8 @@ swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float fr
return out; return out;
} }
swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right) { // Set the target spin for each wheel swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right) // Set the target spin for each wheel
{
swerve_drive out = input; swerve_drive out = input;
out.front_left_target_spin = front_left + input.spin_offset; out.front_left_target_spin = front_left + input.spin_offset;
out.front_right_target_spin = front_right + input.spin_offset; out.front_right_target_spin = front_right + input.spin_offset;
@ -172,7 +193,8 @@ swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_rig
return out; return out;
} }
swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) { // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed
{
swerve_drive out = input; swerve_drive out = input;
float delta_spin_offset = new_spin_offset - input.spin_offset; float delta_spin_offset = new_spin_offset - input.spin_offset;
@ -184,7 +206,8 @@ swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) { // Set a
return out; return out;
} }
swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) { // Set a new drive power swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) // Set a new drive power
{
swerve_drive out = input; swerve_drive out = input;
out.target_drive_power = target_drive_power; out.target_drive_power = target_drive_power;
return out; return out;

4
src/swerve.h
View File

@ -85,6 +85,10 @@ float calculateSteeringMotorSpeed(float steering_delta); // Calculate the 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 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
int normalizeEncoderData(int encoder_data); // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
float relativeAngleFromEncoder(int encoder_data_relative ); // Calculate the relative angle from the difference between 2 encoder data points
float normalizeAngle(float angle); // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees float normalizeAngle(float angle); // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right); // Set the motor speed coefficients for each motor swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right); // Set the motor speed coefficients for each motor

4
src/zserio.cpp
View File

@ -81,8 +81,7 @@ void comm_parse() {
} }
} }
} }
// TESTING: temporarily disabled for motor testing on 20230929
/*
if(millis()-ptime > FAILTIME){ if(millis()-ptime > FAILTIME){
//digitalWrite(13,LOW); //digitalWrite(13,LOW);
SerCommDbg.println("No input recieved, sending safe inputs"); SerCommDbg.println("No input recieved, sending safe inputs");
@ -90,5 +89,4 @@ void comm_parse() {
memcpy(astate,&safe,sizeof(packet_t)); memcpy(astate,&safe,sizeof(packet_t));
comm_ok=0; comm_ok=0;
} }
*/
} }