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
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

70
src/main.cpp
View File

@ -6,13 +6,13 @@
#include <SPI.h>
#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 <Sabertooth.h>
#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);