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

Basic, untested swerve drive outputs

Browse Source
This commit is contained in:
Cole Deck 2023-09-27 12:35:48 -05:00
parent 48d9bcef30
commit e12337eb16
1 changed files with 42 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/main.cpp
View File

@ -36,10 +36,17 @@ byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0
PCF8574 ioex1(0x20, 20, 21); PCF8574 ioex1(0x20, 20, 21);
PCF8574 ioex2(0x21, 20, 21); PCF8574 ioex2(0x21, 20, 21);
PioEncoder enc1(18); // right PioEncoder enc1(18); // right1
PioEncoder enc2(14); // left PioEncoder enc2(14); // left1
PioEncoder enc3(26); // right2
PioEncoder enc4(0); // left2 // JANK: soldered to pico or headers
// stepper board slave config // pins for arm servos
#define ARM_SERVO_PIN_1 2
#define ARM_SERVO_PIN_2 3
#define ARM_SERVO_PIN_3 8
// stepper board slave pins
#define MOTOR_X_ENABLE_PIN 8 #define MOTOR_X_ENABLE_PIN 8
#define MOTOR_X_STEP_PIN 2 #define MOTOR_X_STEP_PIN 2
#define MOTOR_X_DIR_PIN 5 #define MOTOR_X_DIR_PIN 5
@ -68,10 +75,10 @@ Sabertooth actuators(130, Serial2);
#define MAX_MICROS 1985 #define MAX_MICROS 1985
#define OC_OFFSET 0 // microseconds #define OC_OFFSET 0 // microseconds
#define SERVO_PIN_1 5 #define TALON_PIN_1 5
#define SERVO_PIN_2 6 #define TALON_PIN_2 6
#define SERVO_PIN_3 7 #define TALON_PIN_3 7
#define SERVO_PIN_4 9 #define TALON_PIN_4 9
typedef struct typedef struct
{ {
@ -80,11 +87,11 @@ typedef struct
} ISR_servo_t; } ISR_servo_t;
#define NUM_SERVOS 4 #define NUM_SERVOS 7
ISR_servo_t ISR_servo[NUM_SERVOS] = ISR_servo_t ISR_servo[NUM_SERVOS] =
{ {
{ -1, SERVO_PIN_1 }, { -1, SERVO_PIN_2 }, { -1, SERVO_PIN_3 }, { -1, SERVO_PIN_4 } { -1, TALON_PIN_1 }, { -1, TALON_PIN_2 }, { -1, TALON_PIN_3 }, { -1, TALON_PIN_4 }, { -1, ARM_SERVO_PIN_1 }, { -1, ARM_SERVO_PIN_2 }, { -1, ARM_SERVO_PIN_3 }
}; };
MCP3008 adc; MCP3008 adc;
@ -120,6 +127,9 @@ int olddisplay = 99999; // guarantee a change when first value comes in
#define LIFT2 12 #define LIFT2 12
#define LIFT3 13 #define LIFT3 13
#define LIFTALL 14 #define LIFTALL 14
#define ARMSERVO1 15
#define ARMSERVO2 16
#define ARMSERVO3 17
unsigned int getButton(unsigned int num) { unsigned int getButton(unsigned int num) {
if (num <= 7) { if (num <= 7) {
@ -379,7 +389,8 @@ void set_motor(byte motor, int speed) {
// 5 - 8 : drive motors on Talon SRX // 5 - 8 : drive motors on Talon SRX
// 9 - 10 : actuators on Sabertooth 3 // 9 - 10 : actuators on Sabertooth 3
// 11 - 13 : Steppers on slave board // 11 - 13 : Steppers on slave board
// 14 : Drive 11-13 with identical position & speed // 14 : drive 11-13 with identical position & speed
// 15 - 17 : arm servos
// speed is -127 to 127 // speed is -127 to 127
Serial.print("Driving motor "); Serial.print("Driving motor ");
Serial.print(motor); Serial.print(motor);
@ -457,6 +468,10 @@ void set_motor(byte motor, int speed) {
ioex1.digitalWrite(2, HIGH); // disable ioex1.digitalWrite(2, HIGH); // disable
} }
} }
else if (motor < 18) { // arm servos
uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET);
RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 15 + 4].servoIndex, newspeed);
}
} }
void setup() { void setup() {
@ -864,6 +879,9 @@ void loop() {
int zeroed_rx = -1 * ((int)(astate->stickRX) - 127); int zeroed_rx = -1 * ((int)(astate->stickRX) - 127);
int zeroed_ry = ((int)(astate->stickRY) - 127); int zeroed_ry = ((int)(astate->stickRY) - 127);
// Modulus swerve drive
if (sqrt(zeroed_lx^2 + zeroed_ly^2) > 10) { // if left stick is touched if (sqrt(zeroed_lx^2 + zeroed_ly^2) > 10) { // if left stick is touched
drive_mode = drive_type; drive_mode = drive_type;
} }
@ -872,9 +890,6 @@ void loop() {
} }
// Modulus swerve drive
if (drive_mode == DRIVE_BASIC) { if (drive_mode == DRIVE_BASIC) {
swrv = tempBasicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx)); swrv = tempBasicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
} }
@ -884,6 +899,19 @@ void loop() {
if (drive_mode == DRIVE_ROTATION) { if (drive_mode == DRIVE_ROTATION) {
swrv = tempRotationDrive(swrv, zeroed_rx); swrv = tempRotationDrive(swrv, zeroed_rx);
} }
swrv = updateSwerveCommand(swrv);
// update motors after calculation
set_motor(FLSTEER, swrv.front_left_command_spin);
set_motor(BRSTEER, swrv.back_right_command_spin);
set_motor(FRSTEER, swrv.front_right_command_spin);
set_motor(BLSTEER, swrv.back_left_command_spin);
set_motor(FLDRIVE, swrv.current_drive_power);
set_motor(BRDRIVE, swrv.current_drive_power);
set_motor(FRDRIVE, swrv.current_drive_power);
set_motor(BLDRIVE, swrv.current_drive_power);
// Goliath / 2 side arcade tank drive code below // Goliath / 2 side arcade tank drive code below
/*int zeroed_power = -1 * ((int)(astate->stickX) - 127); /*int zeroed_power = -1 * ((int)(astate->stickX) - 127);