update
This commit is contained in:
Cole Deck
parent
e601e5a57c
commit
59be703a36
49
src/main.cpp
49
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);
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user