got motor running

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 uint16_t maxPulseLength
-    const float initialPosition
 ) 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)
+    const float hardwareTarget = virtualAngleToHardwareAngle(target);
-    float delta = currentHardwarePosition;
+    currentHardwarePosition = constrain(hardwareTarget, safeHardwareMinAngle, safeHardwareMaxAngle);
-    currentHardwarePosition = virtualAngleToHardwareAngle(target);
-    delta = abs(delta - currentHardwarePosition);
-
-    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;
+    // True if they are the same, e.g. no clamping was applied
-    }
+    return (abs(currentHardwarePosition - hardwareTarget) < COMP_EPSILON);
-    else {
-        return false;
-    }
 }
 
 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;
 }

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 uint16_t maxPulseLength = 625
-            const float initialPosition = 0
         ) 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 uint16_t minPulseLength;
-        const uint8_t maxPulseLength;
+        const uint16_t maxPulseLength;
 };

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[0] = ((int16_t)packet[1] << 8) | packet[2];
-    command.args[1] = ((uint16_t)packet[3] << 8) | packet[4];
+    command.args[1] = ((int16_t)packet[3] << 8) | packet[4];
 
     // Validate instruction
     if ((uint8_t)command.instruction > 0x02) {

CommandParser.hpp

@@ -5,6 +5,7 @@
 // Singleton-class
 class CommandParser
 {
+    public:
         // Each command is 5 bytes long (assuming no padding)
         struct Command {
             enum class TYPE : uint8_t {
@@ -12,18 +13,16 @@ class CommandParser
                 MOVE_RELATIVE   = 0x01,
                 RESET_POSITIONS = 0x02,
             } instruction;
-        uint16_t args[2];
+            int16_t args[2];
         };
         
-    public:
-    
         static CommandParser& getInstance() 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();

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);
+        }
+    }
 }

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;
 };

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;
 }

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

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);
+    }
 }

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 = "";

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));
+        // ALso this simplifies recognizing packet ends
-            inBufPos += bytesRead;
+        size_t bytesRead = Serial.readBytes(inBuf + inBufPos, 1);
-        }
+        if (!bytesRead) {
-        // Else, these bytes would overflow the buffer.
+            StateMachine::getInstance().raiseError("Unable to read available byte!");
-        // Read as much as we can and go on. It may just be multiple packets queued.
+            return;
-        else {
-            size_t bytesRead = Serial.readBytes(inBuf + inBufPos, min(PACKET_MAX_BYTES_READ_AT_ONCE, PACKET_SIZE - inBufPos));
-            inBufPos += bytesRead;
         }
+        inBufPos++;
 
-        // Is the last byte read 0x0A? Then push the packet
+        // Is our buffer full? Push it.
-        if (inBufPos > 0 && inBuf[inBufPos - 1] == TERMINATOR_BYTE) {
+        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?");
-        }
     }
 }
 

UartHandler.hpp

@@ -2,8 +2,7 @@
 #include <Arduino.h>
 
 // Will parse serial messages using the serial interface supplied by the dev board
-// Expects packets of at max 16 bytes.
+// Expects packets must be 5 bytes.
-// Terminating byte must be 0x0A.
 // 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 = 5;
-        static constexpr uint8_t PACKET_SIZE = 16;
         static constexpr uint8_t PACKET_RINGBUF_SIZE = 16;
 
     private:

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};
+*/

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();
 }