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

Add new servo driver (untested)

Browse Source
This commit is contained in:
Cole Deck 2023-09-28 22:02:33 -05:00
parent 2b254aaab4
commit 9701aa4187
2 changed files with 30 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
platformio.ini
View File

@ -13,7 +13,7 @@ platform = https://github.com/maxgerhardt/platform-raspberrypi.git
board = rpipicow board = rpipicow
framework = arduino framework = arduino
platform_packages = 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 upload_port = /mnt/pico
board_build.core = earlephilhower board_build.core = earlephilhower
lib_deps = lib_deps =
@ -23,6 +23,6 @@ lib_deps =
ftjuh/I2Cwrapper@^0.5.0 ftjuh/I2Cwrapper@^0.5.0
waspinator/AccelStepper@^1.64 waspinator/AccelStepper@^1.64
olikraus/Ucglib@^1.5.2 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 debug_tool = cmsis-dap
build_flags = -O3 ; optimize build level 3 build_flags = -O3

68
src/main.cpp
View File

@ -6,13 +6,13 @@
#include <SPI.h> #include <SPI.h>
#include "MCP3XXX.h" #include "MCP3XXX.h"
#include "PCF8574.h" #include "PCF8574.h"
#include "RP2040_ISR_Servo.h"
#include "pio_encoder.h" #include "pio_encoder.h"
//#include "dumbdisplay.h" //#include "dumbdisplay.h"
//#include "wifidumbdisplay.h" //#include "wifidumbdisplay.h"
#include "zserio.h" #include "zserio.h"
#include "SerialUART.h" #include "SerialUART.h"
#include <Sabertooth.h> #include <Sabertooth.h>
#include <107-Arduino-Servo-RP2040.h>
#include "swerve.h" #include "swerve.h"
#include "manipulator.h" #include "manipulator.h"
@ -89,6 +89,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
static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3;
typedef struct typedef struct
{ {
int servoIndex; int servoIndex;
@ -98,11 +100,6 @@ typedef struct
#define NUM_SERVOS 7 #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; MCP3008 adc;
int count = 0; int count = 0;
int mode = 1; int mode = 1;
@ -409,15 +406,14 @@ void set_motor(byte motor, int speed) {
// swerve controls // swerve controls
swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed); swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
} }
else if (motor <= 8) { else if (motor == 5)
// Talon SRX drive talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
// Note: can't use the built in arduino-pico Servo.h because it uses the PIO - which is already full else if (motor == 6)
// Can't use PWM because that would slow the PWM frequency to 50hz, kinda need more than that talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
// So we use hardware interrupt timers instead else if (motor == 7)
//RP2040_ISR_Servos.setPosition(ISR_servo[motor - 5].servoIndex, position); talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET); else if (motor == 8)
RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 5].servoIndex, newspeed); talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
}
else if (motor <= 10) { else if (motor <= 10) {
// actuator controls // actuator controls
actuators.motor((motor - 9 + 1), speed); actuators.motor((motor - 9 + 1), speed);
@ -477,10 +473,12 @@ void set_motor(byte motor, int speed) {
ioex1.digitalWrite(2, HIGH); // disable ioex1.digitalWrite(2, HIGH); // disable
} }
} }
else if (motor < 18) { // arm servos else if (motor == 15)
uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET); arm1.writeMicroseconds(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 == 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() { void setup() {
@ -659,31 +657,17 @@ void setup() {
actuators.setTimeout(100); actuators.setTimeout(100);
// Talon SRX init // Servo & Talon SRX init
for (int index = 0; index < NUM_SERVOS; index++) // talon0, talon1, talon2, talon3, arm1, arm2, arm3
{ talon1.attach(TALON_PIN_1);
digitalWrite(ISR_servo[index].servoPin, LOW); talon2.attach(TALON_PIN_2);
pinMode(ISR_servo[index].servoPin, OUTPUT); talon3.attach(TALON_PIN_3);
digitalWrite(ISR_servo[index].servoPin, LOW); talon4.attach(TALON_PIN_4);
}
for (int index = 0; index < NUM_SERVOS; index++) arm1.attach(ARM_SERVO_PIN_1);
{ arm2.attach(ARM_SERVO_PIN_2);
ISR_servo[index].servoIndex = RP2040_ISR_Servos.setupServo(ISR_servo[index].servoPin, MIN_MICROS, MAX_MICROS); arm3.attach(ARM_SERVO_PIN_3);
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);
Serial.println(" done"); Serial.println(" done");
delay(20); delay(20);