leonetienne/MinumelArmMCU
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

got motor running

Browse Source
This commit is contained in:
Leonetienne
2025-12-14 18:57:28 +01:00
parent d339f5b297
commit bd906add53
14 changed files with 123 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
AdafruitServoMotor.cpp
+20 -21
View File
@@ -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);
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;
// Only move the motor if it would turn it at least n degrees
if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) {
commitMoveImmediately();
}
// 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;
}
AdafruitServoMotor.hpp
+5 -4
View File
@@ -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;
};
CommandParser.cpp
+2 -2
View File
@@ -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) {
CommandParser.hpp
+13 -14
View File
@@ -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:
// 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;
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();
MainLoop.cpp
+35 -3
View File
@@ -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
+5
View File
@@ -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
+2 -2
View File
@@ -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
+1 -1
View File
@@ -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
+12 -1
View File
@@ -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
+1 -1
View File
@@ -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
+10 -22
View File
@@ -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;
}
// 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;
// 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;
}
inBufPos++;
// Is the last byte read 0x0A? Then push the packet
if (inBufPos > 0 && inBuf[inBufPos - 1] == TERMINATOR_BYTE) {
// 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?");
}
}
}
UartHandler.hpp
+2 -4
View File
@@ -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.
// 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:
defs.hpp
+2
View File
@@ -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
+10
View File
@@ -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();
}