From bd906add53aa18e4884ce5463a9b6f16c1f32360 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Sun, 14 Dec 2025 18:57:28 +0100 Subject: [PATCH] got motor running --- AdafruitServoMotor.cpp | 43 +++++++++++++++++++++--------------------- AdafruitServoMotor.hpp | 9 +++++---- CommandParser.cpp | 4 ++-- CommandParser.hpp | 29 ++++++++++++++-------------- MainLoop.cpp | 38 ++++++++++++++++++++++++++++++++++--- MainLoop.hpp | 5 +++++ RangedMotor.cpp | 4 ++-- RangedMotor.hpp | 2 +- StateMachine.cpp | 13 ++++++++++++- StateMachine.hpp | 2 +- UartHandler.cpp | 34 +++++++++++---------------------- UartHandler.hpp | 6 ++---- defs.hpp | 2 ++ minumel_arm_clean.ino | 10 ++++++++++ 14 files changed, 123 insertions(+), 78 deletions(-) diff --git a/AdafruitServoMotor.cpp b/AdafruitServoMotor.cpp index 372f1b8..bf015e7 100644 --- a/AdafruitServoMotor.cpp +++ b/AdafruitServoMotor.cpp @@ -1,9 +1,10 @@ #include "AdafruitServoMotor.hpp" #include "StateMachine.hpp" +#include "defs.hpp" // Motor hardware angle must be at least changed by that much before // the motion is commited to the motor (prevent motor rattle) -constexpr float MIN_DELTA_FOR_MOTION_COMMIT = 0.02f; +constexpr float MIN_DELTA_FOR_MOTION_COMMIT = 0.01f; AdafruitServoMotor::AdafruitServoMotor( AdafruitPWMBoard& driver, @@ -14,9 +15,9 @@ AdafruitServoMotor::AdafruitServoMotor( const float minSafeAngle, const float maxSafeAngle, const float specRangeDeg, + const float initialPosition, const uint16_t minPulseLength, - const uint16_t maxPulseLength, - const float initialPosition + const uint16_t maxPulseLength ) noexcept: driver(driver), motorIndex(motorIndex), @@ -41,7 +42,6 @@ AdafruitServoMotor::AdafruitServoMotor( StateMachine::getInstance().raiseError("Servo motor received offline driver board!"); } - if (!driver.occupyMotorSlot(motorIndex)) { StateMachine::getInstance().raiseError("Servo motor index already occupied for adafruit pwm board!"); } @@ -53,28 +53,24 @@ bool AdafruitServoMotor::moveTo(const float target) noexcept StateMachine::getInstance().raiseError("Servo motor driver board gone offline!"); } - // delta = abs(old_hardware_pos - new_hardware_pos) - float delta = currentHardwarePosition; - currentHardwarePosition = virtualAngleToHardwareAngle(target); - delta = abs(delta - currentHardwarePosition); + const float hardwareTarget = virtualAngleToHardwareAngle(target); + currentHardwarePosition = constrain(hardwareTarget, safeHardwareMinAngle, safeHardwareMaxAngle); + + // Only move the motor if it would turn it at least n degrees + if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) { + commitMoveImmediately(); + } - if (currentHardwarePosition >= safeHardwareMinAngle && - currentHardwarePosition <= safeHardwareMaxAngle) { - - // Only move the motor if it would turn it at least n degrees - if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) { - commitMoveImmediately(); - } - - return true; - } - else { - return false; - } + // True if they are the same, e.g. no clamping was applied + return (abs(currentHardwarePosition - hardwareTarget) < COMP_EPSILON); } void AdafruitServoMotor::commitMoveImmediately() noexcept { + if (!driver.isGood()) { + StateMachine::getInstance().raiseError("Servo motor driver board gone offline!"); + } + driver.get().setPWM(motorIndex, 0, hardwareAngleToPulse(currentHardwarePosition)); lastCommittedHardwarePosition = currentHardwarePosition; return false; @@ -82,5 +78,8 @@ void AdafruitServoMotor::commitMoveImmediately() noexcept uint16_t AdafruitServoMotor::hardwareAngleToPulse(const float hardwareAngle) const noexcept { - return map(hardwareAngle, 0, specRangeDeg, minPulseLength, maxPulseLength); + const float t = hardwareAngle / specRangeDeg; + const uint16_t pulse = minPulseLength + t * (maxPulseLength - minPulseLength); + + return pulse; } diff --git a/AdafruitServoMotor.hpp b/AdafruitServoMotor.hpp index 9c4bbfa..d57b34b 100644 --- a/AdafruitServoMotor.hpp +++ b/AdafruitServoMotor.hpp @@ -15,12 +15,13 @@ class AdafruitServoMotor : public RangedMotor const float minSafeAngle, const float maxSafeAngle, const float specRangeDeg, // 180 for a 180-deg-servo, 270 for a 270-deg-servo, etc ... + const float initialPosition = 0, const uint16_t minPulseLength = 125, - const uint16_t maxPulseLength = 125, - const float initialPosition = 0 + const uint16_t maxPulseLength = 625 ) noexcept; // Returns true if the motion is accepted, false if it is outside of safe bounds. + // If out of safe bounds, maximum or minimun safe value will be assumen bool moveTo(const float target) noexcept override; float getPosition() const noexcept override { return hardwareAngleToVirtualAngle(currentHardwarePosition); }; @@ -34,6 +35,6 @@ class AdafruitServoMotor : public RangedMotor float currentHardwarePosition = 0; float lastCommittedHardwarePosition = 0; float specRangeDeg; - const uint8_t minPulseLength; - const uint8_t maxPulseLength; + const uint16_t minPulseLength; + const uint16_t maxPulseLength; }; diff --git a/CommandParser.cpp b/CommandParser.cpp index dc599c0..370389f 100755 --- a/CommandParser.cpp +++ b/CommandParser.cpp @@ -13,8 +13,8 @@ CommandParser::Command CommandParser::parsePacket(const uint8_t *packet) noexcep Command command; command.instruction = (Command::TYPE)packet[0]; // Fetch args by big endian byte ordering - command.args[0] = ((uint16_t)packet[1] << 8) | packet[2]; - command.args[1] = ((uint16_t)packet[3] << 8) | packet[4]; + command.args[0] = ((int16_t)packet[1] << 8) | packet[2]; + command.args[1] = ((int16_t)packet[3] << 8) | packet[4]; // Validate instruction if ((uint8_t)command.instruction > 0x02) { diff --git a/CommandParser.hpp b/CommandParser.hpp index 2c4c777..cd218d3 100755 --- a/CommandParser.hpp +++ b/CommandParser.hpp @@ -5,25 +5,24 @@ // Singleton-class class CommandParser { - // Each command is 5 bytes long (assuming no padding) - struct Command { - enum class TYPE : uint8_t { - MOVE_ABSOLUTE = 0x00, - MOVE_RELATIVE = 0x01, - RESET_POSITIONS = 0x02, - } instruction; - uint16_t args[2]; - }; - public: - - static CommandParser& getInstance() noexcept; + // Each command is 5 bytes long (assuming no padding) + struct Command { + enum class TYPE : uint8_t { + MOVE_ABSOLUTE = 0x00, + MOVE_RELATIVE = 0x01, + RESET_POSITIONS = 0x02, + } instruction; + int16_t args[2]; + }; + + static CommandParser& getInstance() noexcept; - // Expecting command to be a packet of max size UartHandler::PACKET_SIZE - Command parsePacket(const uint8_t* packet) noexcept; + // Expecting command to be a packet of max size UartHandler::PACKET_SIZE + Command parsePacket(const uint8_t* packet) noexcept; private: - CommandParser() noexcept; + CommandParser() noexcept {}; CommandParser(const CommandParser&) = delete; CommandParser(CommandParser&&) = delete; ~CommandParser(); diff --git a/MainLoop.cpp b/MainLoop.cpp index d1bf398..11e3cbb 100755 --- a/MainLoop.cpp +++ b/MainLoop.cpp @@ -1,8 +1,26 @@ #include "MainLoop.hpp" +#include "StateMachine.hpp" +#include "CommandParser.hpp" -MainLoop::MainLoop() noexcept +MainLoop::MainLoop() noexcept: + pwmBoard(0x40, 60), + m0( + pwmBoard, + 0, + 1.0, + 1.0, + 135.0, + -135.0, + 45.0, + 180.0, + 0, + 172, + 565 + ) { - UartHandler::getInstance().init(9600); + // Initialize state machine singleton + StateMachine::getInstance(); + //m0.setScaleCalibration(RangedMotor::calculateScaleCalibration(90, 95)); } MainLoop::~MainLoop() noexcept @@ -18,9 +36,23 @@ MainLoop& MainLoop::getInstance() noexcept void MainLoop::setup() noexcept { + UartHandler::getInstance().init(9600); + m0.commitMoveImmediately(); } void MainLoop::update() noexcept { - UartHandler::getInstance().poll(); + UartHandler& uart = UartHandler::getInstance(); + uart.poll(); + + while(uart.getNumAvailablePackets()) { + uint8_t packet[16]; + uart.getNextPacket(packet); + CommandParser::Command command = CommandParser::getInstance().parsePacket(packet); + + if (command.instruction == CommandParser::Command::TYPE::MOVE_ABSOLUTE) { + const float moveTarget = (float)command.args[1] / 10.0f; + m0.moveTo(moveTarget); + } + } } diff --git a/MainLoop.hpp b/MainLoop.hpp index 512709c..afdcf74 100755 --- a/MainLoop.hpp +++ b/MainLoop.hpp @@ -1,5 +1,7 @@ #pragma once #include "UartHandler.hpp" +#include "AdafruitPWMBoard.hpp" +#include"AdafruitServoMotor.hpp" // Singleton-instance class MainLoop @@ -16,4 +18,7 @@ class MainLoop MainLoop() noexcept; ~MainLoop() noexcept; + + AdafruitPWMBoard pwmBoard; + AdafruitServoMotor m0; }; diff --git a/RangedMotor.cpp b/RangedMotor.cpp index d3e8fba..d89ff12 100755 --- a/RangedMotor.cpp +++ b/RangedMotor.cpp @@ -66,7 +66,7 @@ void RangedMotor::setGearboxRatio(float gearboxRatio) noexcept void RangedMotor::setScaleCalibration(float scaleCalibration) noexcept { - if (abs(gearboxRatio) < COMP_EPSILON) { + if (abs(scaleCalibration) < COMP_EPSILON) { StateMachine::getInstance().raiseError("Zero-scale-calibration was set on ranged motor!"); return; } @@ -129,5 +129,5 @@ float RangedMotor::hardwareAngleToVirtualAngle(float hardwareAngle) const noexce float RangedMotor::calculateScaleCalibration(const float virtualRange, const float hardwareRange) { - return virtualRange / hardwareRange; + return hardwareRange / virtualRange; } diff --git a/RangedMotor.hpp b/RangedMotor.hpp index 5a7fe0c..214b4b5 100755 --- a/RangedMotor.hpp +++ b/RangedMotor.hpp @@ -37,12 +37,12 @@ public: // Will translate hardware-space-angles to virtual-space-angles float hardwareAngleToVirtualAngle(float hardwareAngle) const noexcept; -protected: // Calculates a command-space scale compensation factor. // The returned value is multiplied with virtual angle so that // the motors actual movement matches the intended movement. static float calculateScaleCalibration(const float virtualRange, const float hardwareRange); +protected: float gearboxRatio; // Gearbox ratio float scaleCalibration; // Scale calibration float zeroOffset; // Zero offset calibration diff --git a/StateMachine.cpp b/StateMachine.cpp index 963569c..1ef8ff5 100755 --- a/StateMachine.cpp +++ b/StateMachine.cpp @@ -1,5 +1,13 @@ #include "StateMachine.hpp" +constexpr uint8_t ERROR_INDICATOR_LED_PIN = 3; + +StateMachine::StateMachine() noexcept +{ + pinMode(ERROR_INDICATOR_LED_PIN, OUTPUT); + digitalWrite(ERROR_INDICATOR_LED_PIN, LOW); +} + StateMachine& StateMachine::getInstance() noexcept { static StateMachine instance; @@ -77,9 +85,12 @@ bool StateMachine::canSwitchToState(const State newState) const noexcept return false; } - bool StateMachine::raiseError(const String &reason) noexcept { state = State::ERROR; lastError = reason; + digitalWrite(ERROR_INDICATOR_LED_PIN, HIGH); + if (Serial) { + Serial.println(String("Error!: ") + reason); + } } diff --git a/StateMachine.hpp b/StateMachine.hpp index 646f97e..b7ca4f7 100755 --- a/StateMachine.hpp +++ b/StateMachine.hpp @@ -24,7 +24,7 @@ class StateMachine bool canSwitchToState(const State newState) const noexcept; private: - StateMachine() noexcept {}; + StateMachine() noexcept; State state = State::INITIALIZING; String lastError = ""; diff --git a/UartHandler.cpp b/UartHandler.cpp index cd0a478..c04dde1 100755 --- a/UartHandler.cpp +++ b/UartHandler.cpp @@ -2,8 +2,6 @@ #include "OperationTimeout.hpp" #include "StateMachine.hpp" -constexpr uint8_t PACKET_MAX_BYTES_READ_AT_ONCE = 4; - UartHandler::UartHandler() noexcept { } @@ -50,32 +48,22 @@ bool UartHandler::isOpen() const noexcept void UartHandler::poll() { - int bytesAvail = Serial.available(); + size_t 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; + // Consume at max one byte at a time to give consumers a chance to consume packets + // ALso this simplifies recognizing packet ends + size_t bytesRead = Serial.readBytes(inBuf + inBufPos, 1); + if (!bytesRead) { + StateMachine::getInstance().raiseError("Unable to read available byte!"); + return; } - // 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] == TERMINATOR_BYTE) { + inBufPos++; + + // Is our buffer full? Push it. + if (inBufPos == PACKET_SIZE) { 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] != TERMINATOR_BYTE) { - StateMachine::getInstance().raiseError("UART packet did not contain the 0x0A endbyte! May it be too long?"); - } } } diff --git a/UartHandler.hpp b/UartHandler.hpp index 679978b..c17d4a0 100755 --- a/UartHandler.hpp +++ b/UartHandler.hpp @@ -2,8 +2,7 @@ #include // 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. +// Expects packets must be 5 bytes. // Singleton-class class UartHandler { @@ -27,8 +26,7 @@ class UartHandler bool isOpen() const noexcept; - static constexpr uint8_t TERMINATOR_BYTE = 0x0A; - static constexpr uint8_t PACKET_SIZE = 16; + static constexpr uint8_t PACKET_SIZE = 5; static constexpr uint8_t PACKET_RINGBUF_SIZE = 16; private: diff --git a/defs.hpp b/defs.hpp index 1876ab2..2a6a222 100755 --- a/defs.hpp +++ b/defs.hpp @@ -3,6 +3,7 @@ constexpr float COMP_EPSILON = 0.01; // ----- Servo config ----- +/* #define SERVO_PMIN 125 // Minimum pulse length (0 degrees) #define SERVO_PMAX 625 // Maximum pulse length (180 degrees) @@ -11,3 +12,4 @@ 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}; +*/ \ No newline at end of file diff --git a/minumel_arm_clean.ino b/minumel_arm_clean.ino index 067b24e..671d293 100755 --- a/minumel_arm_clean.ino +++ b/minumel_arm_clean.ino @@ -3,6 +3,16 @@ MainLoop* mainloop = nullptr; void setup() { + // Play small init sequence on error LED to let users know that the device is (re-)booting + pinMode(3, OUTPUT); + for (int i = 0; i < 4; i++) { + digitalWrite(3, HIGH); + delay(100); + digitalWrite(3, LOW); + delay(100); + } + + // Init system mainloop = &MainLoop::getInstance(); mainloop->setup(); }