Duplicated rpipico for pico2 (acutators)

One servo is at least working, should be able to handle more, needs testing

rpi-pico-actuators/include/commands/commands.hpp

@@ -5,7 +5,7 @@
 #ifndef COMMANDS_HPP
 #define COMMANDS_HPP
 
-#define READ_ENCODERS  'e'
+#define SERVO_CMD  's'
 #define MOTOR_SPEEDS   'm'
 #define ACCELERATION   'a'
 #define MAX_SPEED      's'

rpi-pico-actuators/include/pins/pins.hpp

@@ -1,17 +1,17 @@
-#define ENABLE_PIN 2
+#include <array>
 
-#define CFG1_PIN 16
-#define CFG2_PIN 17
-
-#define DIR_PIN1 14
-#define STEP_PIN1 15
-
-
-#define DIR_PIN2 12
-#define STEP_PIN2 13
-
-#define DIR_PIN3 10
-#define STEP_PIN3 11
-
-#define DIR_PIN4 8
-#define STEP_PIN4 9
+constexpr std::array<int, 13> servoGpioNumbers = {
+    /*placeholder*/ 0,
+    /* servo 1 */ 2,
+    /* servo 2 */ 3,
+    /* servo 3 */ 6,
+    /* servo 4 */ 7,
+    /* servo 5 */ 8,
+    /* servo 6 */ 9,
+    /* servo 7 */ 10,
+    /* servo 8 */ 11,
+    /* servo 9 */ 12,
+    /* servo 10 */ 13,
+    /* servo 11 */ 14,
+    /* servo 12 */ 14
+};

rpi-pico-actuators/include/servo/servo_actuator.hpp

@@ -0,0 +1,29 @@
+#ifndef SERVOACTUATOR_HPP
+#define SERVOACTUATOR_HPP
+
+
+#include <Arduino.h>
+#include <Servo.h>
+
+#include <array>
+
+
+class ServoActuator {
+    public:
+        ServoActuator(int gpioNumber);
+        ServoActuator(int gpoNumber, int min, int max, int safe);
+        void write(int angle);
+    private:
+        int gpioNumber;
+        int min;
+        int max;
+        int safe;
+        Servo servo;
+        int limit(int angle);
+
+
+
+};
+
+
+#endif

rpi-pico-actuators/platformio.ini

@@ -13,3 +13,4 @@ platform = raspberrypi
 board = pico
 framework = arduino
 monitor_speed = 115200
+lib_deps = arduino-libraries/Servo@^1.2.1

rpi-pico-actuators/src/main.cpp

@@ -2,22 +2,16 @@
 
 #include <hardware/uart.h>
 
-#include <stepper/stepper.hpp>
+#include <servo/servo_actuator.hpp>
 #include "commands/commands.hpp"
 #include "pins/pins.hpp"
 
 UART Serial2(4,5, NC, NC);
 
-// AccelStepper stepper1(AccelStepper::DRIVER, STEP_PIN1, DIR_PIN1);
-// AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN2, DIR_PIN2);
-// AccelStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_PIN3);
-// AccelStepper stepper4(AccelStepper::DRIVER, STEP_PIN4, DIR_PIN4);
+ServoActuator servo1(servoGpioNumbers[1], 20, 180, 90);
 
+ServoActuator servos[] = {servo1};
 
-stepper  stepper1 = stepper(STEP_PIN1, DIR_PIN1, ENABLE_PIN, 0, 0, 2000, 1000);
-stepper  stepper2 = stepper(STEP_PIN2, DIR_PIN2, ENABLE_PIN, 0, 0, 2000, 1000);
-stepper  stepper3 = stepper(STEP_PIN3, DIR_PIN3, ENABLE_PIN, 0, 0, 2000, 1000);
-stepper  stepper4 = stepper(STEP_PIN4, DIR_PIN4, ENABLE_PIN, 0, 0, 2000, 1000);
 
 int arg = 0;
 int i = 0;
@@ -37,36 +31,14 @@ long arg3;
 long arg4;
 
 
+
 void setup() {
 
   Serial.begin(115200);
   Serial2.begin(115200);
   Serial.println("Hello World !");
 
 
-  stepper  stepper1 = stepper(STEP_PIN1, DIR_PIN1, ENABLE_PIN, 0, 0, 1000, 10000);
-  stepper  stepper2 = stepper(STEP_PIN2, DIR_PIN2, ENABLE_PIN, 0, 0, 1000, 10000);
-  stepper  stepper3 = stepper(STEP_PIN3, DIR_PIN3, ENABLE_PIN, 0, 0, 1000, 10000);
-  stepper  stepper4 = stepper(STEP_PIN4, DIR_PIN4, ENABLE_PIN, 0, 0, 1000, 10000);
-
-
-  // pin mode setup
-  pinMode(ENABLE_PIN, OUTPUT);
-  pinMode(CFG1_PIN, OUTPUT);
-  pinMode(CFG2_PIN, OUTPUT);
-
-
-  // enable the stepper drivers
-  digitalWrite(ENABLE_PIN, LOW);
-
-  // microstepping option : 
-  // Currently 1/8th steps
-  // https://wiki.fysetc.com/TMC2208/#motor-current-setting
-  // MS1 <-> CFG1
-  // MS2 <-> CFG2
-  digitalWrite(CFG1_PIN, LOW);
-  digitalWrite(CFG2_PIN, LOW);
-
 
 }
 
@@ -95,45 +67,23 @@ int runCommand(){
   arg4 = atoi(argv4);
 
   switch(cmd){
-    case MOTOR_SPEEDS:
-      Serial.print("MOTOR_SPEEDS ");
+    case SERVO_CMD:
+      Serial.print("SERVO number : ");
       Serial.print(arg1);
-      Serial.print(" ");
+      Serial.print(" angle : ");
       Serial.print(arg2);
-      Serial.print(" ");
-      Serial.print(arg3);
-      Serial.print(" ");
-      Serial.println(arg4);
-
-
-      //run the actual steppers
-      stepper1.setTargetSpeed(arg1);
-      stepper2.setTargetSpeed(arg2);
-      stepper3.setTargetSpeed(arg3);
-      stepper4.setTargetSpeed(arg4);
-
 
+      if (arg1 >= 1 && arg1 <= 12){
+        servos[arg1-1].write(arg2);
+      }
+      else {
+        Serial.print("Invalid servo number: ");
+        Serial.println(arg1);
+      }
 
       break;
-    case ACCELERATION:
-      Serial.print("ACCELERATION ");
-      Serial.print(arg1);
 
-      stepper1.setAcceleration(arg1);
-      stepper2.setAcceleration(arg1);
-      stepper3.setAcceleration(arg1);
-      stepper4.setAcceleration(arg1);
-      break;
-    case MAX_SPEED:
-      Serial.print("MAX_SPEED ");
-      Serial.print(arg1);
 
-      stepper1.setMaxSpeed(arg1);
-      stepper2.setMaxSpeed(arg1);
-      stepper3.setMaxSpeed(arg1);
-      stepper4.setMaxSpeed(arg1);
-      break;
-    // TODO : add a command for sending back the "encoder" values
     default:
       Serial.print("Unknown command: ");
       Serial.println(cmd);
@@ -148,15 +98,8 @@ void loop() {
 
   //run speed each motor
 
-  stepper1.run();
-  stepper2.run();
-  stepper3.run();
-  stepper4.run();
-
-  stepper2.log();
-
-  while(Serial2.available() > 0){
-    chr = Serial2.read();
+  while(Serial.available() > 0){
+    chr = Serial.read();
 
     // if the chr is a carriage return
     if (chr == '\r'){

rpi-pico-actuators/src/servo.cpp

@@ -0,0 +1,34 @@
+#include "servo/servo_actuator.hpp"
+
+
+ServoActuator::ServoActuator(int gpioNumber) {
+    this->gpioNumber = gpioNumber;
+    this->servo.attach(gpioNumber);
+}
+
+
+ServoActuator::ServoActuator(int gpioNumber, int min, int max, int safe) {
+    this->gpioNumber = gpioNumber;
+    this->min = min;
+    this->max = max;
+    this->safe = safe;
+    this->servo.attach(gpioNumber);
+    this->servo.write(safe);
+
+}
+
+int ServoActuator::limit(int angle) {
+    if (angle > max) {
+        return max;
+    } else if (angle < min) {
+        return min;
+    } else {
+        return angle;
+    }
+}
+
+
+void ServoActuator::write(int angle) {
+    this->servo.write(limit(angle));
+}
+