Swerve control working

Co-authored-by: Amelia Deck <cdeck@hawk.iit.edu>

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

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_STOP 0
-#define DRIVE_TRANSLATION 1
+#define DRIVE_BASIC 1
-#define DRIVE_ROTATION 2
+#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

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
+void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_float, float zeroed_ry_float, int drive_mode, \
-  char buffer[200];
+               float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude) { // Print encoder positions for steering motors
-  sprintf(buffer, "ENC1 = %i\nENC2 = %i\nENC3 = $i\nENC4 = %i\n", enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount());
+  
-  SerComm.print(buffer);
+  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_lx =    ((int)(astate->stickX) - 127);
-  int zeroed_ly =      ((int)(astate->stickY) - 127);
+  int zeroed_ly =    -1 * ((int)(astate->stickY) - 127);
-  int zeroed_rx =    -1 * ((int)(astate->stickRX) - 127);
+  int zeroed_rx =     ((int)(astate->stickRX) - 127);
-  int zeroed_ry =      ((int)(astate->stickRY) - 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;
 
-  if (sqrt(zeroed_lx^2 + zeroed_ly^2) > 10) { // if left stick is touched
+  // Joystick calculations
-    drive_mode = drive_type;
+  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 (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
+  if (fabs(right_joystick_magnitude) > 0.1 ) { // if right stick is touched - override translation
-    drive_mode = DRIVE_ROTATION;
+    loop_drive_mode = DRIVE_ROTATION;
   }
 
-  
+  // Select operation based on current drive mode
-  if (drive_mode == DRIVE_BASIC) {
+  switch(loop_drive_mode) {
-    swrv = basicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
+    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);
 }

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,6 +145,15 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
     return out;
 }
 
+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;

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