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

update

Browse Source
This commit is contained in:
Cole Deck 2023-09-30 10:52:31 -05:00
parent e601e5a57c
commit 59be703a36
3 changed files with 31 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/main.cpp
View File

@ -44,8 +44,8 @@ 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(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929 PioEncoder enc2(0); // Front Right
PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929 PioEncoder enc3(2); // Back Left
PioEncoder enc4(14); // Back Right PioEncoder enc4(14); // Back Right
@ -103,8 +103,8 @@ Sabertooth actuators(130, Serial2);
#define TALON_PIN_3 7 #define TALON_PIN_3 7
#define TALON_PIN_4 9 #define TALON_PIN_4 9
// pins for arm servos // pins for arm servos
#define ARM_SERVO_PIN_1 2 #define ARM_SERVO_PIN_1 26
#define ARM_SERVO_PIN_2 3 #define ARM_SERVO_PIN_2 27
#define ARM_SERVO_PIN_3 8 #define ARM_SERVO_PIN_3 8
static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3; static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3;
@ -439,7 +439,8 @@ void set_motor(byte motor, int speed) {
//stepperX.setSpeed((float)speed); //stepperX.setSpeed((float)speed);
if (abs(speed) > 0) if (abs(speed) > 0)
ioex1.digitalWrite(2, LOW); // enable ioex1.digitalWrite(2, LOW); // enable
else
stepperX_pos = stepperX.currentPosition();
stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperX_pos = speed * 96 + stepperX.currentPosition(); stepperX_pos = speed * 96 + stepperX.currentPosition();
@ -451,7 +452,8 @@ void set_motor(byte motor, int speed) {
//stepperY.setSpeed((float)speed); //stepperY.setSpeed((float)speed);
if (abs(speed) > 0) if (abs(speed) > 0)
ioex1.digitalWrite(2, LOW); // enable ioex1.digitalWrite(2, LOW); // enable
else
stepperY_pos = stepperY.currentPosition();
stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperY_pos = speed * 96 + stepperY.currentPosition(); stepperY_pos = speed * 96 + stepperY.currentPosition();
@ -463,6 +465,8 @@ void set_motor(byte motor, int speed) {
//stepperY.setSpeed((float)speed); //stepperY.setSpeed((float)speed);
if (abs(speed) > 0) if (abs(speed) > 0)
ioex1.digitalWrite(2, LOW); // enable ioex1.digitalWrite(2, LOW); // enable
else
stepperZ_pos = stepperZ.currentPosition();
stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperZ_pos = speed * 96 + stepperZ.currentPosition(); stepperZ_pos = speed * 96 + stepperZ.currentPosition();
@ -486,6 +490,15 @@ void set_motor(byte motor, int speed) {
stepperZ.runState(); stepperZ.runState();
} else { } else {
ioex1.digitalWrite(2, HIGH); // disable ioex1.digitalWrite(2, HIGH); // disable
stepperX_pos = stepperX.currentPosition();
stepperX.setCurrentPosition(stepperX_pos);
stepperY.setCurrentPosition(stepperX_pos);
stepperZ.setCurrentPosition(stepperX_pos);
stepperX.stop();
stepperY.stop();
stepperZ.stop();
} }
} }
else if (motor == 15) else if (motor == 15)
@ -594,19 +607,11 @@ void setup() {
ioex1.digitalWrite(3, HIGH); ioex1.digitalWrite(3, HIGH);
delay(2000); delay(2000);
digitalWrite(ALI1, LOW);
digitalWrite(BLI1, LOW);
digitalWrite(AHI1, LOW);
digitalWrite(BHI1, LOW);
digitalWrite(ALI2, LOW); digitalWrite(ALI2, LOW);
digitalWrite(BLI2, LOW); digitalWrite(BLI2, LOW);
digitalWrite(AHI2, LOW); digitalWrite(AHI2, LOW);
digitalWrite(BHI2, LOW); digitalWrite(BHI2, LOW);
pinMode(ALI1, OUTPUT);
pinMode(AHI1, OUTPUT);
pinMode(BLI1, OUTPUT);
pinMode(BHI1, OUTPUT);
pinMode(ALI2, OUTPUT); pinMode(ALI2, OUTPUT);
pinMode(AHI2, OUTPUT); pinMode(AHI2, OUTPUT);
pinMode(BLI2, OUTPUT); pinMode(BLI2, OUTPUT);
@ -699,7 +704,9 @@ void setup() {
enc1.begin(); enc1.begin();
//enc1.flip(); //enc1.flip();
enc2.begin(); enc2.begin();
//enc2.flip();
enc3.begin(); enc3.begin();
//enc3.flip();
enc4.begin(); enc4.begin();
Serial.println(" done"); Serial.println(" done");
delay(20); delay(20);
@ -973,14 +980,14 @@ void loop() {
swrv.enable_steering = false; swrv.enable_steering = false;
break; break;
} }
swrv = updateEncoderData(swrv, enc1.getCount(), enc3.getCount(), enc4.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct 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 swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
//DEBUG TESTING code: //DEBUG TESTING code:
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, normalizeAngle(swrv.front_right_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("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("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)); Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, normalizeAngle(swrv.back_left_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 // 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 float arm_speed = (float) (((int) getButton(DPAD_UP)) - ((int) getButton(DPAD_DOWN))); // TODO 20230929 confirm speed and polarity
@ -1010,19 +1017,19 @@ void loop() {
// update motors after calculation // update motors after calculation
// TESTING 20230929 comment out to test steering // TESTING 20230929 comment out to test steering
/*
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);
*/
// Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers // Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
spinlock_lock_core_0(&drive_power_command_spinlock_flag); spinlock_lock_core_0(&drive_power_command_spinlock_flag);
power_data_transfer_fl = swrv.front_left_spin_power; power_data_transfer_fl = swrv.front_left_spin_power;
// TESTING 20230929 comment out since encoders not yet connected // TESTING 20230929 comment out since encoders not yet connected
//power_data_transfer_fr = swrv.front_right_spin_power; power_data_transfer_fr = swrv.front_right_spin_power;
//power_data_transfer_bl = swrv.back_left_spin_power; power_data_transfer_bl = swrv.back_left_spin_power;
power_data_transfer_br = swrv.back_right_spin_power; power_data_transfer_br = swrv.back_right_spin_power;
spinlock_release(&drive_power_command_spinlock_flag); spinlock_release(&drive_power_command_spinlock_flag);

2
src/manipulator.cpp
View File

@ -106,7 +106,7 @@ manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the a
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));
out.arm_speed = arm_speed; out.arm_speed = arm_speed;
out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed))); out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed))) * 2;
return out; return out;
} }
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

4
src/swerve.cpp
View File

@ -70,8 +70,8 @@ swerve_drive updateSwerveCommand(swerve_drive input)
// TESTING DEBUG print 20230929 // TESTING DEBUG print 20230929
Serial.printf("FL delta = %f\t\tBR delta = %f\r\n", front_left_delta, back_right_delta); Serial.printf("FR delta = %f\t\tBL delta = %f\r\n", front_right_delta, back_left_delta);
Serial.printf("FL steer = %f\t\tBR steer = %f\r\n", out.front_left_spin_power, out.back_right_spin_power); Serial.printf("FR steer = %f\t\tBL steer = %f\r\n", out.front_right_spin_power, out.back_left_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;