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

Swerve control working

Browse Source 
Co-authored-by: Amelia Deck <cdeck@hawk.iit.edu>
This commit is contained in:
evlryah 2023-09-28 00:29:44 -05:00
parent 785cc6b5db
commit 4a0e19eeac
5 changed files with 96 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
platformio.ini
View File

@ -14,7 +14,7 @@ board = rpipicow
framework = arduino
platform_packages =
framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master ; use master branch for setXXX fix
upload_port = /run/media/amelia/RPI-RP2/
upload_port = /mnt/pico
board_build.core = earlephilhower
lib_deps =
xreef/PCF8574 library@^2.3.6

12
src/globals.h
View File

@ -26,9 +26,13 @@ extern long last_p;
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
// Drive modes
#define DRIVE_BASIC 0
#define DRIVE_TRANSLATION 1
#define DRIVE_ROTATION 2
#define DRIVE_STOP 0
#define DRIVE_BASIC 1
#define DRIVE_TRANSLATION 2
#define DRIVE_ROTATION 3
// Controller maximum inputs for joystick
#define CONTROLLER_JOYSTICK_MAX 128.0
// Basic mode conditions, specifies which direction and turning direction the robot is using
#define DRIVE_BASIC_STOP 0
@ -45,7 +49,7 @@ extern long last_p;
#define ENCODER_BUFFER_ENTRY_COUNT 5
// Number of encoder ticks per degree of rotation for the swerve drive steering motors
#define STEERING_ENCODER_TICKS_PER_DEGREE 1.0 // TODO as of 20230927
#define STEERING_ENCODER_TICKS_PER_DEGREE (1024.0 * 4.0) / 360.0 // TODO check as of 20230927
// 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

108
src/main.cpp
View File

@ -23,7 +23,7 @@ const char* password = "pink4bubble";
//JoystickDDLayer *appjoystick;
swerve_drive swrv;
bool drive_type = DRIVE_TRANSLATION;
// bool drive_type = DRIVE_TRANSLATION;
int drive_mode = DRIVE_TRANSLATION;
packet_t pA, pB, safe;
@ -67,6 +67,7 @@ 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
uint64_t loop_counter = 0; // Counter for loop() , incremented at the end of each loop
#define defMaxSpeed 8000
#define defAcceleration 8000
@ -401,7 +402,7 @@ void set_motor(byte motor, int speed) {
Serial.println(speed);
if (motor <= 4) {
// swerve controls
swivel[(motor - 1) / 2].motor((motor - 1) % 2, speed);
swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
}
else if (motor <= 8) {
// Talon SRX drive
@ -414,7 +415,7 @@ void set_motor(byte motor, int speed) {
}
else if (motor <= 10) {
// actuator controls
actuators.motor((motor - 9), speed);
actuators.motor((motor - 9 + 1), speed);
}
// set servos
// speed needs to be a high number to be reasonable
@ -646,7 +647,7 @@ void setup() {
// Sabertooth init
Serial2.setTX(4);
Serial2.setRX(5);
Serial2.begin(9600);
Serial2.begin(2400); // Sabertooths on modulus have been reprogrammed to 2400 baud!
Sabertooth::autobaud(Serial2);
//Sabertooth::autobaud();
@ -687,6 +688,8 @@ void setup() {
set_hex(0x5);
//enc1.begin();
//enc2.begin();
//enc3.begin();
//enc4.begin();
Serial.println(" done");
delay(20);
@ -698,6 +701,8 @@ void setup() {
comm_init(); //Initialize the communication FSM
safe.stickX = 127;
safe.stickY = 127;
safe.stickRX = 127;
safe.stickRY = 127;
safe.btnhi = 0;
safe.btnlo = 0;
safe.cksum = 0b1000000010001011;
@ -863,10 +868,22 @@ void print_status() {
}
void print_encoder_positions() { // Print encoder positions for steering motors
char buffer[200];
sprintf(buffer, "ENC1 = %i\nENC2 = %i\nENC3 = $i\nENC4 = %i\n", enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount());
SerComm.print(buffer);
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
char buffer[300] = "\0";
// Create a new line to show the start of this telemetry cycle
//Serial.println(buffer);
//SerComm.println(buffer);
// 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", \
loop_counter, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
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);
Serial.println(buffer);
SerComm.println(buffer);
}
void loop() {
@ -875,11 +892,6 @@ void loop() {
rp2040.wdt_reset();
comm_parse();
if (getButton(SHOULDER_TOP_RIGHT))
drive_type = DRIVE_TRANSLATION;
else if(getButton(SHOULDER_TOP_LEFT))
drive_type = DRIVE_BASIC;
//const DDFeedback* fb;
/*if (!dumbdisplay.connected() || !WiFi.isConnected()) {
target_left_power = 0;
@ -889,43 +901,66 @@ void loop() {
} else
fb = appjoystick->getFeedback();*/
int zeroed_lx = -1 * ((int)(astate->stickX) - 127);
int zeroed_ly = ((int)(astate->stickY) - 127);
int zeroed_rx = -1 * ((int)(astate->stickRX) - 127);
int zeroed_ry = ((int)(astate->stickRY) - 127);
int zeroed_lx = ((int)(astate->stickX) - 127);
int zeroed_ly = -1 * ((int)(astate->stickY) - 127);
int zeroed_rx = ((int)(astate->stickRX) - 127);
int zeroed_ry = -1 * ((int)(astate->stickRY) - 127);
// Modulus swerve drive
// Modify controller joystick inputs to be within a range of -1.0 to 1.0
float zeroed_lx_float = ((float) zeroed_lx) / CONTROLLER_JOYSTICK_MAX;
float zeroed_ly_float = ((float) zeroed_ly) / CONTROLLER_JOYSTICK_MAX;
float zeroed_rx_float = ((float) zeroed_rx) / CONTROLLER_JOYSTICK_MAX;
float zeroed_ry_float = ((float) zeroed_ry) / CONTROLLER_JOYSTICK_MAX;
// Joystick calculations
float left_joystick_magnitude = sqrt(pow(zeroed_lx_float,2) + pow(zeroed_ly_float,2));
float left_joystick_angle = fmod(360.0 - fmod((DEGREES_PER_RADIAN * atan2(zeroed_ly_float, zeroed_lx_float)) + 270.0, 360.0) , 360.0);
float right_joystick_magnitude = sqrt(pow(zeroed_rx_float,2) + pow(zeroed_ry_float,2));
float right_joystick_angle = fmod(360.0 - fmod((DEGREES_PER_RADIAN * atan2(zeroed_ry_float, zeroed_rx_float)) + 270.0, 360.0) , 360.0);
// add 270, fmod 360, 360 - n, fmod 360
// Drive mode calculation
int loop_drive_mode = DRIVE_STOP; // Temporary drive mode specific to this loop, is DRIVE_STOP (0) until it is changed
if (getButton(SHOULDER_TOP_RIGHT))
drive_mode = DRIVE_TRANSLATION;
else if(getButton(SHOULDER_TOP_LEFT))
drive_mode = DRIVE_BASIC;
if (sqrt(zeroed_lx^2 + zeroed_ly^2) > 10) { // if left stick is touched
drive_mode = drive_type;
if (fabs(left_joystick_magnitude) > 0.1 ) { // if left stick is touched
loop_drive_mode = drive_mode;
}
if (sqrt(zeroed_rx^2 + zeroed_ry^2) > 10) { // if right stick is touched - override translation
drive_mode = DRIVE_ROTATION;
if (fabs(right_joystick_magnitude) > 0.1 ) { // if right stick is touched - override translation
loop_drive_mode = DRIVE_ROTATION;
}
if (drive_mode == DRIVE_BASIC) {
swrv = basicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
// Select operation based on current drive mode
switch(loop_drive_mode) {
case DRIVE_STOP:
swrv = stopSwerve(swrv);
break;
case DRIVE_BASIC:
swrv = basicDrive(swrv, left_joystick_magnitude, left_joystick_angle);
break;
case DRIVE_TRANSLATION:
swrv = translationDrive(swrv, left_joystick_magnitude, left_joystick_angle);
break;
case DRIVE_ROTATION:
swrv = rotationDrive(swrv, zeroed_rx_float);
break;
}
if (drive_mode == DRIVE_TRANSLATION) {
swrv = translationDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
}
if (drive_mode == DRIVE_ROTATION) {
swrv = rotationDrive(swrv, zeroed_rx);
}
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
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
// update motors after calculation
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(BRDRIVE, swrv.back_right_power);
set_motor(FRDRIVE, swrv.front_right_power);
@ -1078,9 +1113,10 @@ void loop() {
//delay(200);
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
if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS
delay(delay_time_ms);
}
loop_counter++;
//DDYield();
}
@ -1153,7 +1189,7 @@ void loop1() {
mode = 1;
}
}
for (int x = 5; x < 9; x++) {
/*for (int x = 5; x < 9; x++) {
set_motor(x, count);
}
swivel[0].motor(0, 127);
@ -1164,7 +1200,7 @@ void loop1() {
digitalWrite(0, HIGH);
digitalWrite(1, HIGH);
digitalWrite(2, HIGH);
digitalWrite(3, HIGH);
digitalWrite(3, HIGH);*/
delay(25);
}

14
src/swerve.cpp
View File

@ -72,6 +72,9 @@ swerve_drive updateSwerveCommand(swerve_drive input)
out.back_right_spin_power = 0.0f;
}
// 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;
// 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;
@ -142,10 +145,19 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
return out;
}
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
swerve_drive stopSwerve(swerve_drive input) // Stop all motors
{
swerve_drive out = input;
out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
out = setDriveTargetPower(out, 0.0); // Set the power
return out;
}
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
{
swerve_drive out = input;
float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); // Set the target angle for each rotation motor

2
src/swerve.h
View File

@ -83,6 +83,8 @@ 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 stopSwerve(swerve_drive input); // Stop all motors
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode
swerve_drive rotationDrive(swerve_drive input, float target_speed); // Implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise