From 9701aa41878df6bbbad3cf3f2d0ca80fa0a0cb12 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Thu, 28 Sep 2023 22:02:33 -0500 Subject: [PATCH] Add new servo driver (untested) --- platformio.ini | 6 ++--- src/main.cpp | 70 +++++++++++++++++++------------------------------- 2 files changed, 30 insertions(+), 46 deletions(-) diff --git a/platformio.ini b/platformio.ini index 1e7633c..0d45a6a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -13,7 +13,7 @@ platform = https://github.com/maxgerhardt/platform-raspberrypi.git board = rpipicow framework = arduino platform_packages = - framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master ; use master branch for setXXX fix + framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master upload_port = /mnt/pico board_build.core = earlephilhower lib_deps = @@ -23,6 +23,6 @@ lib_deps = ftjuh/I2Cwrapper@^0.5.0 waspinator/AccelStepper@^1.64 olikraus/Ucglib@^1.5.2 - khoih-prog/RP2040_ISR_Servo@^1.1.2 + 107-systems/107-Arduino-Servo-RP2040@^0.1.0 debug_tool = cmsis-dap -build_flags = -O3 ; optimize build level 3 +build_flags = -O3 diff --git a/src/main.cpp b/src/main.cpp index cb72e7b..ad322f0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,13 +6,13 @@ #include #include "MCP3XXX.h" #include "PCF8574.h" -#include "RP2040_ISR_Servo.h" #include "pio_encoder.h" //#include "dumbdisplay.h" //#include "wifidumbdisplay.h" #include "zserio.h" #include "SerialUART.h" #include +#include <107-Arduino-Servo-RP2040.h> #include "swerve.h" #include "manipulator.h" @@ -89,6 +89,8 @@ Sabertooth actuators(130, Serial2); #define TALON_PIN_3 7 #define TALON_PIN_4 9 +static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3; + typedef struct { int servoIndex; @@ -98,11 +100,6 @@ typedef struct #define NUM_SERVOS 7 -ISR_servo_t ISR_servo[NUM_SERVOS] = -{ - { -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; int count = 0; int mode = 1; @@ -409,15 +406,14 @@ void set_motor(byte motor, int speed) { // swerve controls swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed); } - else if (motor <= 8) { - // Talon SRX drive - // Note: can't use the built in arduino-pico Servo.h because it uses the PIO - which is already full - // Can't use PWM because that would slow the PWM frequency to 50hz, kinda need more than that - // So we use hardware interrupt timers instead - //RP2040_ISR_Servos.setPosition(ISR_servo[motor - 5].servoIndex, position); - uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET); - RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 5].servoIndex, newspeed); - } + else if (motor == 5) + talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); + else if (motor == 6) + talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); + else if (motor == 7) + talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); + else if (motor == 8) + talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); else if (motor <= 10) { // actuator controls actuators.motor((motor - 9 + 1), speed); @@ -477,10 +473,12 @@ void set_motor(byte motor, int speed) { 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); - } + else if (motor == 15) + arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); + else if (motor == 16) + arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); + else if (motor == 17) + arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); } void setup() { @@ -659,31 +657,17 @@ void setup() { actuators.setTimeout(100); - // Talon SRX init - for (int index = 0; index < NUM_SERVOS; index++) - { - digitalWrite(ISR_servo[index].servoPin, LOW); - pinMode(ISR_servo[index].servoPin, OUTPUT); - digitalWrite(ISR_servo[index].servoPin, LOW); - } + // Servo & Talon SRX init + // talon0, talon1, talon2, talon3, arm1, arm2, arm3 + talon1.attach(TALON_PIN_1); + talon2.attach(TALON_PIN_2); + talon3.attach(TALON_PIN_3); + talon4.attach(TALON_PIN_4); - for (int index = 0; index < NUM_SERVOS; index++) - { - ISR_servo[index].servoIndex = RP2040_ISR_Servos.setupServo(ISR_servo[index].servoPin, MIN_MICROS, MAX_MICROS); - - if (ISR_servo[index].servoIndex != -1) - { - //Serial.print(F("Setup OK Servo index = ")); Serial.println(ISR_servo[index].servoIndex); - - RP2040_ISR_Servos.setPosition(ISR_servo[index].servoIndex, 0); - } - else - { - Serial.print(F("Setup Failed Servo index = ")); Serial.println(ISR_servo[index].servoIndex); - } - } - for (int i = 5; i < 9; i++) - set_motor(i, 0); + arm1.attach(ARM_SERVO_PIN_1); + arm2.attach(ARM_SERVO_PIN_2); + arm3.attach(ARM_SERVO_PIN_3); + Serial.println(" done"); delay(20);