Merge branch 'modulus-plus' of https://git.myitr.org/adeck/rib-test-code into modulus-plus

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

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,6 +90,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;
@@ -99,11 +101,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;
@@ -410,15 +407,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);
@@ -478,10 +474,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() {
@@ -660,31 +658,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);
+  arm1.attach(ARM_SERVO_PIN_1);
+  arm2.attach(ARM_SERVO_PIN_2);
+  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");
   delay(20);