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