feat: began working on cleaning up explorative main file

MainLoop.cpp

@@ -0,0 +1,26 @@
+#include "MainLoop.hpp"
+
+MainLoop::MainLoop() noexcept
+{
+    UartHandler::getInstance().init(9600);
+}
+
+MainLoop::~MainLoop() noexcept
+{
+    UartHandler::getInstance().close();
+}
+
+MainLoop& MainLoop::getInstance() noexcept
+{
+    static MainLoop instance;
+    return instance;
+}
+
+void MainLoop::setup() noexcept
+{
+}
+
+void MainLoop::update() noexcept
+{
+    UartHandler::getInstance().poll();
+}

MainLoop.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include "UartHandler.hpp"
+
+// Singleton-instance
+class MainLoop
+{ 
+    public:
+        MainLoop(const MainLoop&) = delete;
+        MainLoop(MainLoop&&) = delete;
+        static MainLoop& getInstance() noexcept;
+        void setup() noexcept;
+        void update() noexcept;
+
+    private:
+        MainLoop() noexcept;
+        ~MainLoop() noexcept;
+};

OperationTimeout.cpp

@@ -0,0 +1,36 @@
+#include "OperationTimeout.hpp"
+
+OperationTimeout::OperationTimeout(const unsigned long maxMs) noexcept:
+    maxMs(maxMs)
+{
+    start = millis();
+}
+
+OperationTimeout::OperationTimeout(const long maxMs) noexcept:
+    maxMs(maxMs)
+{
+    start = millis();
+}
+
+OperationTimeout::OperationTimeout(const unsigned int maxMs) noexcept:
+    maxMs(maxMs)
+{
+    start = millis();
+}
+
+OperationTimeout::OperationTimeout(const int maxMs) noexcept:
+    maxMs(maxMs)
+{
+    start = millis();
+}
+
+bool OperationTimeout::isTimeout() const noexcept
+{
+    // Assuming millis() will not overflow
+    return (millis() - start) > maxMs;
+}
+
+void OperationTimeout::restart() noexcept
+{
+    start = millis();
+}

OperationTimeout.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include <Arduino.h>
+
+// Will alert after a set time has passed
+class OperationTimeout
+{
+    public:
+    OperationTimeout(const unsigned long maxMs) noexcept;
+    OperationTimeout(const long maxMs) noexcept;
+    OperationTimeout(const unsigned int maxMs) noexcept;
+    OperationTimeout(const int maxMs) noexcept;
+    OperationTimeout(const OperationTimeout&) = delete;
+    OperationTimeout(OperationTimeout&&) noexcept = default;
+
+    bool isTimeout() const noexcept;
+    void restart() noexcept;
+
+    private:
+    unsigned long start;
+    const unsigned long maxMs;
+};

StateMachine.cpp

@@ -0,0 +1,85 @@
+#include "StateMachine.hpp"
+
+StateMachine& StateMachine::getInstance() noexcept
+{
+    static StateMachine instance;
+    return instance;
+}
+
+bool StateMachine::setState(const State newState) noexcept
+{
+    if (canSwitchToState(newState)) {
+        state = newState;
+        return true;
+    }
+
+    return false;
+}
+
+bool StateMachine::canSwitchToState(const State newState) const noexcept
+{
+    // Error state is final
+    if (state == State::ERROR) {
+        return false;
+    }
+
+    // Error state only settable via raiseError()
+    if (newState == State::ERROR) {
+        return false;
+    }
+
+    // Allow same-to-same transition
+    if (state == newState) {
+        return true;
+    }
+
+    switch (state) {
+        case StateMachine::State::INITIALIZING:
+            switch (newState) {
+                // Can go to idle
+                case State::IDLING:
+                    return true;
+                // Can't go straight to moving
+                case State::MOVING:
+                    return false;
+
+                default:
+                    return false;
+            }
+        break;
+        case StateMachine::State::IDLING:
+            switch (newState) {
+                // Can't initialize again
+                case State::INITIALIZING:
+                    return false;
+                // Can enter move state
+                case State::MOVING:
+                    return true;
+
+                default:
+                    return false;
+            }
+        break;
+        case StateMachine::State::MOVING:
+            switch (newState) {
+                // Can't initialize again
+                case State::INITIALIZING:
+                    return false;
+                // Can go into idling
+                case State::IDLING:
+                    return true;
+
+                default:
+                    return false;
+            }
+        break;
+    }
+    return false;
+}
+
+
+bool StateMachine::raiseError(const String &reason) noexcept
+{
+    state = State::ERROR;
+    lastError = reason;
+}

StateMachine.hpp

@@ -0,0 +1,31 @@
+#pragma once
+#include <Arduino.h>
+
+// Singleton-instance
+class StateMachine
+{
+    public:
+    StateMachine(const StateMachine&) = delete;
+    StateMachine(StateMachine&&) = delete;
+    static StateMachine& getInstance() noexcept;
+
+     enum class State {
+        INITIALIZING,
+        IDLING,
+        MOVING,
+        ERROR
+    };
+
+    State getState() const noexcept { return state; };
+    String getLastError() const noexcept { return lastError; };
+    // May fail if a state transition from state a to b is not valid
+    bool setState(const State newState) noexcept;
+    bool raiseError(const String& reason) noexcept;
+    bool canSwitchToState(const State newState) const noexcept;
+
+    private:
+    StateMachine() noexcept {};
+
+    State state = State::INITIALIZING;
+    String lastError = "";
+};

UartHandler.cpp

@@ -0,0 +1,111 @@
+#include "UartHandler.hpp"
+#include "OperationTimeout.hpp"
+#include "StateMachine.hpp"
+
+constexpr uint8_t PACKET_MAX_BYTES_READ_AT_ONCE = 4;
+
+UartHandler::UartHandler() noexcept
+{
+}
+
+UartHandler::~UartHandler()
+{
+    // In case close was not called, close now
+    if (isOpen()) {
+        close();
+    }
+}
+
+UartHandler &UartHandler::getInstance()
+{
+    static UartHandler instance;
+    return instance;
+}
+
+bool UartHandler::init(const uint32_t baudRate)
+{
+    // Wait for serial port to connect, consider failed after one second
+    Serial.begin(baudRate);
+    OperationTimeout ot(1000);
+    while (!Serial) {
+        if (ot.isTimeout()) {
+            StateMachine::getInstance().raiseError("Unable to open serial port");
+            return false;
+        }
+        yield();
+    }
+
+    return true;
+}
+
+void UartHandler::close()
+{
+    Serial.end();
+}
+
+bool UartHandler::isOpen() const noexcept
+{
+    return (bool)Serial;
+}
+
+void UartHandler::poll()
+{
+    int bytesAvail = Serial.available();
+    if (bytesAvail) {
+        // Available bytes fit within remaining buffer
+        if (bytesAvail <= PACKET_SIZE - inBufPos) {
+            // Consume at max one byte at a time to give consumers a chance to consume packets
+            size_t bytesRead = Serial.readBytes(inBuf + inBufPos, min(PACKET_MAX_BYTES_READ_AT_ONCE, bytesAvail));
+            inBufPos += bytesRead;
+        }
+        // Else, these bytes would overflow the buffer.
+        // Read as much as we can and go on. It may just be multiple packets queued.
+        else {
+            size_t bytesRead = Serial.readBytes(inBuf + inBufPos, min(PACKET_MAX_BYTES_READ_AT_ONCE, PACKET_SIZE - inBufPos));
+            inBufPos += bytesRead;
+        }
+        
+        // Is the last byte read 0x0A? Then push the packet
+        if (inBufPos > 0 && inBuf[inBufPos - 1] == 0x0A) {
+            pushPacket();
+            inBufPos = 0;
+        }
+
+        // Is the last byte NOT 0x0A and we are on the last possibly byte?
+        // Then something is wrong and we are declaring error.
+        if (inBufPos == PACKET_SIZE && inBuf[inBufPos - 1] != 0x0A) {
+            StateMachine::getInstance().raiseError("UART packet did not contain the 0x0A endbyte! May it be too long?");
+        }
+    }
+}
+
+void UartHandler::pushPacket()
+{
+    memset(packetRingBuf + packetRingBufWritePos * PACKET_SIZE, 0x00, PACKET_SIZE);
+    memcpy(packetRingBuf + packetRingBufWritePos * PACKET_SIZE, inBuf, inBufPos);
+    packetRingBufWritePos = (packetRingBufWritePos + 1) % PACKET_RINGBUF_SIZE;
+    numPacketsAvailable++;
+
+    if (numPacketsAvailable >= PACKET_RINGBUF_SIZE) {
+        // Move machine into error state -> reports errors and failsafes
+        StateMachine::getInstance().raiseError("UART packet dropped because it was not read!");
+    }
+}
+
+bool UartHandler::getNextPacket(char* dest)
+{
+    if (!numPacketsAvailable) {
+        return false;
+    }
+
+    memcpy(dest, packetRingBuf + packetRingBufReadPos * PACKET_SIZE, PACKET_SIZE);
+    packetRingBufReadPos = (packetRingBufReadPos + 1) % PACKET_RINGBUF_SIZE;
+    numPacketsAvailable--;
+    return true;
+}
+
+
+uint8_t UartHandler::getNumAvailablePackets() const
+{
+    return numPacketsAvailable;
+}

UartHandler.hpp

@@ -0,0 +1,49 @@
+#pragma once
+#include <Arduino.h>
+
+constexpr uint8_t PACKET_SIZE = 16;
+constexpr uint8_t PACKET_RINGBUF_SIZE = 16;
+
+// Will parse serial messages using the serial interface supplied by the dev board
+// Expects packets of at max 16 bytes.
+// Terminating byte must be 0x0A.
+// Singleton-class
+class UartHandler
+{
+    public:
+        static UartHandler& getInstance();
+
+        // Will attempt to initialize the serial port, returns status.
+        // May block up to 1000ms.
+        bool init(const uint32_t baudRate);
+
+        void close();
+
+        void poll();
+        // Will write the next available whole line into char* (provide at least 16 bytes!)
+        // Returns whether any data was written.
+        // If data available is longer than 16 bytes, an error will be raised.
+        bool getNextPacket(char* dest);
+
+        // Returns how many packets are ready for reading
+        uint8_t getNumAvailablePackets() const;
+
+        bool isOpen() const noexcept;
+
+    private:
+        // Will move the current contents of inBuf into packetRingBuf. Does NOT reset inBufPos!
+        void pushPacket();
+
+        UartHandler() noexcept;
+        UartHandler(const UartHandler&) = delete;
+        UartHandler(UartHandler&&) = delete;
+        ~UartHandler();
+
+        // By spec, packets will not exceed 16 bytes
+        uint8_t inBuf[PACKET_SIZE];
+        uint8_t inBufPos = 0;
+        uint8_t packetRingBuf[PACKET_RINGBUF_SIZE * PACKET_SIZE];
+        uint8_t packetRingBufReadPos = 0;   // 0 to PACKET_RINGBUF_SIZE
+        uint8_t packetRingBufWritePos = 0;  // 0 to PACKET_RINGBUF_SIZE
+        uint8_t numPacketsAvailable = 0;
+};

defs.hpp

@@ -0,0 +1,11 @@
+#pragma once
+
+// ----- Servo config -----
+#define SERVO_PMIN 125   // Minimum pulse length (0 degrees)
+#define SERVO_PMAX 625   // Maximum pulse length (180 degrees)
+
+const int servo_range[] = {270, 270, 270, 180};
+const int min_safe_angles[] = {10, 0, 0, 0};
+const int max_safe_angles[] = {270, 181, 270, 180};
+const int initial_pose[] = {186, 71, 171, 36};
+int currentPose[] = {0, 0, 0, 0};

minumel_arm_clean.ino

@@ -0,0 +1,12 @@
+#include "MainLoop.hpp"
+
+MainLoop* mainloop = nullptr;
+
+void setup() {
+  mainloop = &MainLoop::getInstance();
+  mainloop->setup();
+}
+
+void loop() {
+  mainloop->update();
+}

minumel_arm_clean_old.txt

@@ -0,0 +1,184 @@
+#include <Adafruit_PWMServoDriver.h>
+#include <SPI.h>
+#include <mcp_can.h>
+#include "defs.hpp"
+
+Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);
+
+int angleToPulse(int theta, int motorIndex) {
+  return map(theta, 0, servo_range[motorIndex], SERVO_PMIN, SERVO_PMAX);
+}
+
+// ----- UART command handling -----
+String inString = "";
+
+String handleUartComm() {
+  if (Serial.available() > 0) {
+    unsigned char inChar = Serial.read();
+    // Read finished procedure
+    if (inChar == '\n') {
+      // Copy return value
+      String retStr = inString;
+
+      // Reset
+      inString = "";
+      return retStr;
+    }
+    // Store byte procedure
+    else {
+      inString += (char)inChar;
+      return "";
+    }
+  } else {
+    return "";
+  }
+}
+
+// ----- CAN config -----
+const int CAN_CS_PIN = 10;           // CS pin for MCP2515
+MCP_CAN CAN0(CAN_CS_PIN);
+
+// Choose depending on your module crystal:
+//   MCP_16MHZ for most shields
+//   MCP_8MHZ for many cheap blue boards
+const byte CAN_CLOCK = MCP_8MHZ;    // change to MCP_8MHZ if needed
+
+// CAN bitrate and ID we listen to
+const unsigned long SERVO_CAN_ID = 0x100;  // must match sender
+
+// ----- String utils -----
+int splitByChar(String in, char c, String* out) {
+  int start = 0;
+  int end;
+  int count = 0;
+
+  while (true) {
+    end = in.indexOf(c, start);
+
+    if (end == -1) {
+      out[count++] = in.substring(start);
+      break;
+    }
+
+    out[count++] = in.substring(start, end);
+    start = end + 1;
+  }
+
+  return count;
+}
+
+// Process a complete command like "M 0 150"
+void processCommand(String command) {
+  command.trim();  // remove any stray whitespace
+
+  if (command.length() == 0) {
+    return;
+  }
+
+  String argv[5];
+  int argc = splitByChar(command, ' ', argv);
+
+  // Expecting command like: MA 0 150
+  // Motor Absolute sets absolute motor position
+  if (argc >= 3 && argv[0] == "MA") {
+    int motorIndex = argv[1].toInt();
+    int theta      = argv[2].toInt();
+
+    // Safety range check
+    if (theta < min_safe_angles[motorIndex]) {
+      // Serial.println("Refusing to move motor to " + String(theta));
+    } 
+    else if (theta > max_safe_angles[motorIndex]) {
+      // Serial.println("Refusing to move motor to " + String(theta));
+    }
+    else {
+      board1.setPWM(motorIndex, 0, angleToPulse(theta, motorIndex));
+      currentPose[motorIndex] = theta;
+    }
+  }
+  // Expecting command like: MR 0 150
+  // Motor Relative sets relatove motor position, e.g. move by 10
+  else if (argc >= 3 && argv[0] == "MR") {
+    int motorIndex = argv[1].toInt();
+    int delta      = argv[2].toInt();
+    int targetPosition = constrain(currentPose[motorIndex] + delta, min_safe_angles[motorIndex], max_safe_angles[motorIndex]);
+
+    if (targetPosition != currentPose[motorIndex]) {
+      Serial.println("Moving motor " + String(argv[1]) + " to " + String(targetPosition));
+      board1.setPWM(motorIndex, 0, angleToPulse(targetPosition, motorIndex));
+      currentPose[motorIndex] = targetPosition;
+    }
+  }
+  // Expecting command like: MR 0 150
+  // Command to reset pose
+  else if (argc == 1 && argv[0] == "R") {
+  takeInitialPose();
+  } else {
+    // Serial.println("Received unknown or malformed command: " + command);
+  }
+}
+
+void takeInitialPose() {
+  for (int motorIndex = 0; motorIndex < 4; motorIndex++) {
+    board1.setPWM(motorIndex, 0, angleToPulse(initial_pose[motorIndex], motorIndex));
+    currentPose[motorIndex] = initial_pose[motorIndex];
+    delay(500);
+  }
+}
+
+void setup() {
+  Serial.begin(9600);
+
+  board1.begin();
+  board1.setPWMFreq(60); // Servos operate at ~60 Hz
+
+  takeInitialPose();
+
+  while (!Serial) {
+    ; // wait for serial port to connect (for native USB boards)
+  }
+
+  // ----- CAN init -----
+  while (CAN0.begin(MCP_ANY, CAN_500KBPS, CAN_CLOCK) != CAN_OK) {
+    Serial.println("CAN init failed, retrying...");
+    delay(200);
+  }
+  CAN0.setMode(MCP_NORMAL);
+  Serial.println("CAN init OK");
+}
+
+void loop() {
+  // ----- UART handling -----
+  String uartCommand = handleUartComm();
+  if (uartCommand != "") {
+    //Serial.println(uartCommand);
+    processCommand(uartCommand);
+  }
+
+  // ----- CAN handling -----
+  // Check if a CAN message is available
+  if (CAN_MSGAVAIL == CAN0.checkReceive()) {
+    unsigned long canId;
+    byte len;
+    byte buf[8];
+
+    // Read CAN frame
+    CAN0.readMsgBuf(&canId, &len, buf);
+
+    // Only accept frames with the expected ID (optional but recommended)
+    if (canId == SERVO_CAN_ID) {
+      // Interpret payload as ASCII string, e.g. "M 0 150"
+      String canCommand = "";
+
+      for (byte i = 0; i < len; i++) {
+        canCommand += (char)buf[i];
+      }
+
+      Serial.println(canCommand);
+      processCommand(canCommand);
+    }
+  }
+
+  // You can add a small delay if you want to chill the loop a bit
+  // delay(1);
+}