From e366cbc7e857ff6cc9aeb97c01e58cdba7a7bb7b Mon Sep 17 00:00:00 2001 From: evlryah Date: Thu, 28 Sep 2023 00:25:22 -0500 Subject: [PATCH] Co-authored-by: Amelia Deck --- platformio.ini | 2 +- src/globals.h | 12 ++++-- src/main.cpp | 108 ++++++++++++++++++++++++++++++++----------------- src/swerve.cpp | 14 ++++++- src/swerve.h | 2 + 5 files changed, 96 insertions(+), 42 deletions(-) diff --git a/platformio.ini b/platformio.ini index 49a873f..1e7633c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -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 diff --git a/src/globals.h b/src/globals.h index 22af91b..ef9d197 100644 --- a/src/globals.h +++ b/src/globals.h @@ -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 diff --git a/src/main.cpp b/src/main.cpp index 2963a8c..6fef469 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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); } \ No newline at end of file diff --git a/src/swerve.cpp b/src/swerve.cpp index 32ab21e..6d07381 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -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 diff --git a/src/swerve.h b/src/swerve.h index f618eec..32b3d39 100644 --- a/src/swerve.h +++ b/src/swerve.h @@ -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