From 59be703a36c4e67359d6c64c4fc28237946197fd Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Sat, 30 Sep 2023 10:52:31 -0500 Subject: [PATCH] update --- src/main.cpp | 49 ++++++++++++++++++++++++++------------------- src/manipulator.cpp | 2 +- src/swerve.cpp | 4 ++-- 3 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index a7fdaac..5245d6c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -44,8 +44,8 @@ PCF8574 ioex2(0x21, 20, 21); // JANK: soldered to pico or headers PioEncoder enc1(18); // Front Left -PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929 -PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929 +PioEncoder enc2(0); // Front Right +PioEncoder enc3(2); // Back Left PioEncoder enc4(14); // Back Right @@ -103,8 +103,8 @@ Sabertooth actuators(130, Serial2); #define TALON_PIN_3 7 #define TALON_PIN_4 9 // pins for arm servos -#define ARM_SERVO_PIN_1 2 -#define ARM_SERVO_PIN_2 3 +#define ARM_SERVO_PIN_1 26 +#define ARM_SERVO_PIN_2 27 #define ARM_SERVO_PIN_3 8 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); if (abs(speed) > 0) ioex1.digitalWrite(2, LOW); // enable - + else + stepperX_pos = stepperX.currentPosition(); stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperX_pos = speed * 96 + stepperX.currentPosition(); @@ -451,7 +452,8 @@ void set_motor(byte motor, int speed) { //stepperY.setSpeed((float)speed); if (abs(speed) > 0) ioex1.digitalWrite(2, LOW); // enable - + else + stepperY_pos = stepperY.currentPosition(); stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperY_pos = speed * 96 + stepperY.currentPosition(); @@ -463,6 +465,8 @@ void set_motor(byte motor, int speed) { //stepperY.setSpeed((float)speed); if (abs(speed) > 0) ioex1.digitalWrite(2, LOW); // enable + else + stepperZ_pos = stepperZ.currentPosition(); stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperZ_pos = speed * 96 + stepperZ.currentPosition(); @@ -486,6 +490,15 @@ void set_motor(byte motor, int speed) { stepperZ.runState(); } else { 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) @@ -594,19 +607,11 @@ void setup() { ioex1.digitalWrite(3, HIGH); delay(2000); - digitalWrite(ALI1, LOW); - digitalWrite(BLI1, LOW); - digitalWrite(AHI1, LOW); - digitalWrite(BHI1, LOW); digitalWrite(ALI2, LOW); digitalWrite(BLI2, LOW); digitalWrite(AHI2, LOW); digitalWrite(BHI2, LOW); - pinMode(ALI1, OUTPUT); - pinMode(AHI1, OUTPUT); - pinMode(BLI1, OUTPUT); - pinMode(BHI1, OUTPUT); pinMode(ALI2, OUTPUT); pinMode(AHI2, OUTPUT); pinMode(BLI2, OUTPUT); @@ -699,7 +704,9 @@ void setup() { enc1.begin(); //enc1.flip(); enc2.begin(); + //enc2.flip(); enc3.begin(); + //enc3.flip(); enc4.begin(); Serial.println(" done"); delay(20); @@ -973,14 +980,14 @@ void loop() { swrv.enable_steering = false; 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 //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("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 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 // TESTING 20230929 comment out to test steering - /* + set_motor(FLDRIVE, swrv.front_left_power); set_motor(BRDRIVE, swrv.back_right_power); set_motor(FRDRIVE, swrv.front_right_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 spinlock_lock_core_0(&drive_power_command_spinlock_flag); power_data_transfer_fl = swrv.front_left_spin_power; // TESTING 20230929 comment out since encoders not yet connected - //power_data_transfer_fr = swrv.front_right_spin_power; - //power_data_transfer_bl = swrv.back_left_spin_power; + power_data_transfer_fr = swrv.front_right_spin_power; + power_data_transfer_bl = swrv.back_left_spin_power; power_data_transfer_br = swrv.back_right_spin_power; spinlock_release(&drive_power_command_spinlock_flag); diff --git a/src/manipulator.cpp b/src/manipulator.cpp index 4b2be96..9e7e7b5 100644 --- a/src/manipulator.cpp +++ b/src/manipulator.cpp @@ -106,7 +106,7 @@ manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the a manipulator_arm out = input; 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_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; } 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 diff --git a/src/swerve.cpp b/src/swerve.cpp index 18a3ae3..1d2b2e8 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -70,8 +70,8 @@ swerve_drive updateSwerveCommand(swerve_drive input) // TESTING DEBUG print 20230929 - Serial.printf("FL delta = %f\t\tBR delta = %f\r\n", front_left_delta, back_right_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 delta = %f\t\tBL delta = %f\r\n", front_right_delta, back_left_delta); + 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 out.front_left_spin_power = 0.0f;