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
+	framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master ; use master branch for setXXX fix
 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
-	107-systems/107-Arduino-Servo-RP2040@^0.1.0
+	khoih-prog/RP2040_ISR_Servo@^1.1.2
 debug_tool = cmsis-dap
-build_flags = -O3
+build_flags = -O3 ; optimize build level 3

src/main.cpp

@@ -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"
 
@@ -90,8 +90,6 @@ 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;
@@ -101,6 +99,11 @@ 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;
@@ -407,14 +410,15 @@ void set_motor(byte motor, int speed) {
     // swerve controls
     swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
   }
-  else if (motor == 5)
+  else if (motor <= 8) {
-    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+    // Talon SRX drive
-  else if (motor == 6)
+    // Note: can't use the built in arduino-pico Servo.h because it uses the PIO - which is already full
-    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+    // Can't use PWM because that would slow the PWM frequency to 50hz, kinda need more than that
-  else if (motor == 7)
+    // So we use hardware interrupt timers instead
-    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+    //RP2040_ISR_Servos.setPosition(ISR_servo[motor - 5].servoIndex, position);
-  else if (motor == 8)
+    uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET);
-    talon1.writeMicroseconds(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 <= 10) {
     // actuator controls
     actuators.motor((motor - 9 + 1), speed);
@@ -474,12 +478,10 @@ void set_motor(byte motor, int speed) {
       ioex1.digitalWrite(2, HIGH); // disable
     }
   }
-  else if (motor == 15)
+  else if (motor < 18) { // arm servos
-    arm1.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 == 16)
+    RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 15 + 4].servoIndex, newspeed);
-    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() {
@@ -658,17 +660,31 @@ void setup() {
   actuators.setTimeout(100);
   
 
-  // Servo & Talon SRX init
+  // Talon SRX init
-  // talon0, talon1, talon2, talon3, arm1, arm2, arm3
+  for (int index = 0; index < NUM_SERVOS; index++)
-  talon1.attach(TALON_PIN_1);
+  {
-  talon2.attach(TALON_PIN_2);
+    digitalWrite(ISR_servo[index].servoPin, LOW);
-  talon3.attach(TALON_PIN_3);
+    pinMode(ISR_servo[index].servoPin, OUTPUT);
-  talon4.attach(TALON_PIN_4);
+    digitalWrite(ISR_servo[index].servoPin, LOW);
+  }
 
-  arm1.attach(ARM_SERVO_PIN_1);
+  for (int index = 0; index < NUM_SERVOS; index++)
-  arm2.attach(ARM_SERVO_PIN_2);
+  {
-  arm3.attach(ARM_SERVO_PIN_3);
+    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);
 
   Serial.println(" done");
   delay(20);