From 181139aae5e737630b097263301457a25eaf07f6 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Sat, 13 Dec 2025 20:23:28 +0100 Subject: [PATCH] feat: began working on cleaning up explorative main file --- MainLoop.cpp | 26 ++++++ MainLoop.hpp | 17 ++++ OperationTimeout.cpp | 36 ++++++++ OperationTimeout.hpp | 21 +++++ StateMachine.cpp | 85 ++++++++++++++++++ StateMachine.hpp | 31 +++++++ UartHandler.cpp | 111 +++++++++++++++++++++++ UartHandler.hpp | 49 ++++++++++ defs.hpp | 11 +++ minumel_arm_clean.ino | 12 +++ minumel_arm_clean_old.txt | 184 ++++++++++++++++++++++++++++++++++++++ 11 files changed, 583 insertions(+) create mode 100755 MainLoop.cpp create mode 100755 MainLoop.hpp create mode 100755 OperationTimeout.cpp create mode 100755 OperationTimeout.hpp create mode 100755 StateMachine.cpp create mode 100755 StateMachine.hpp create mode 100755 UartHandler.cpp create mode 100755 UartHandler.hpp create mode 100755 defs.hpp create mode 100755 minumel_arm_clean.ino create mode 100755 minumel_arm_clean_old.txt diff --git a/MainLoop.cpp b/MainLoop.cpp new file mode 100755 index 0000000..d1bf398 --- /dev/null +++ b/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(); +} diff --git a/MainLoop.hpp b/MainLoop.hpp new file mode 100755 index 0000000..2272846 --- /dev/null +++ b/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; +}; diff --git a/OperationTimeout.cpp b/OperationTimeout.cpp new file mode 100755 index 0000000..c60a62f --- /dev/null +++ b/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(); +} diff --git a/OperationTimeout.hpp b/OperationTimeout.hpp new file mode 100755 index 0000000..7abcf7c --- /dev/null +++ b/OperationTimeout.hpp @@ -0,0 +1,21 @@ +#pragma once +#include + +// 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; +}; diff --git a/StateMachine.cpp b/StateMachine.cpp new file mode 100755 index 0000000..963569c --- /dev/null +++ b/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; +} diff --git a/StateMachine.hpp b/StateMachine.hpp new file mode 100755 index 0000000..091b0bd --- /dev/null +++ b/StateMachine.hpp @@ -0,0 +1,31 @@ +#pragma once +#include + +// 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 = ""; +}; diff --git a/UartHandler.cpp b/UartHandler.cpp new file mode 100755 index 0000000..cdf1080 --- /dev/null +++ b/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; +} diff --git a/UartHandler.hpp b/UartHandler.hpp new file mode 100755 index 0000000..db1ca15 --- /dev/null +++ b/UartHandler.hpp @@ -0,0 +1,49 @@ +#pragma once +#include + +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; +}; diff --git a/defs.hpp b/defs.hpp new file mode 100755 index 0000000..fa616c1 --- /dev/null +++ b/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}; diff --git a/minumel_arm_clean.ino b/minumel_arm_clean.ino new file mode 100755 index 0000000..067b24e --- /dev/null +++ b/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(); +} diff --git a/minumel_arm_clean_old.txt b/minumel_arm_clean_old.txt new file mode 100755 index 0000000..0e0cb23 --- /dev/null +++ b/minumel_arm_clean_old.txt @@ -0,0 +1,184 @@ +#include +#include +#include +#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); +}