fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

* improved hw_config file

Browse source 
* added retract angle to homing
* v1.0.3
This commit is contained in:
0x23 2025-10-22 19:09:09 +02:00
parent 40d292ff46
commit dc0278ea2b
8 changed files with 151 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

34
firmware/MotionControllerRP/src/command_parser/command_parser.cpp
View file

@ -57,6 +57,37 @@ bool GCodeCommand::has_word(char word) const {
return false;
}
int GCodeCommand::get_word_count() const {
int c = 0;
for(int i=0; i<LETTER_COUNT; i++) {
if(std::isnan(word_values[i]))
c++;
}
return c;
}
bool GCodeCommand::contains_unsupported_words(const std::string& supported_words_str) const {
// Parse supported words from the string into a fixed-size lookup table
bool supported[LETTER_COUNT] = {false};
// Parse the supported letters from the comma-separated string
for (size_t i = 0; i < supported_words_str.length(); ++i) {
char c = supported_words_str[i];
if (c >= 'A' && c <= 'Z')
supported[c - 'A'] = true;
}
// Now check which words are used in the command and not supported
for (int i = 0; i < LETTER_COUNT; ++i) {
char word = 'A' + i;
if (has_word(word) && !supported[i]) {
return true; // Found an unsupported word
}
}
return false; // All words used in command are supported
}
//--- CommandParser ---------------------------------------------------------------------
CommandParser::CommandParser() : buffer_index(0), command_processor(nullptr) {
@ -120,6 +151,9 @@ bool CommandParser::parse_line(const char* line) {
// Parse remaining words (e.g., X1.0, Y2.5, F200)
while ((token = strtok_r(nullptr, " ", &saveptr))) {
if (token[0] >= 'A' && token[0] <= 'Z') {
if(token[1] == '\0')
command.set_value(token[0], 0.0f);
else
command.set_value(token[0], strtof(token + 1, nullptr));
} else {
command_processor->send_reply("error: invalid parameter\n");

6
firmware/MotionControllerRP/src/command_parser/command_parser.h
View file

@ -10,6 +10,8 @@
//*** CLASS *****************************************************************************
static constexpr int LETTER_COUNT = 26;
//--- GCodeCommand ----------------------------------------------------------------------
class GCodeCommand {
@ -23,10 +25,12 @@ class GCodeCommand {
float get_value(char word) const;
float get_value(char word, float default_value) const;
bool has_word(char word) const;
int get_word_count() const;
bool contains_unsupported_words(const std::string& supported_words_str) const;
private:
std::string command;
float word_values[26];
float word_values[LETTER_COUNT];
};
//--- ICommandProcessor -----------------------------------------------------------------

60
firmware/MotionControllerRP/src/hw_config.h
View file

@ -1,17 +1,39 @@
#pragma once
#include "utilities/math_constants.h"
//--- MOTORS ------------------------------------------------------------------
// motor pole pair count
// * 100 for 0.9deg stepper motors
// * 50 for 1.8deg stepper motors
#define MOTOR1_POLE_PAIRS 100
#define MOTOR2_POLE_PAIRS 100
#define MOTOR3_POLE_PAIRS 100
constexpr float MOTOR1_POLE_PAIRS = 100;
constexpr float MOTOR2_POLE_PAIRS = 100;
constexpr float MOTOR3_POLE_PAIRS = 100;
#define CALIBRATION_RANGE 95 // degrees from home position
#define CALIBRATION_FIELD_VELOCITY 40.0f
//--- ENCODERS ----------------------------------------------------------------
#define HOMING_VELOCITY 1.0f // rad per s
#define HOMING_CURRENT 0.15f // range 0..1
#define HOMING_FINISH_POS 0.5f // in rad
// Conversion factor from encoder angle (one 2pi period every two magnets) to rotor angle.
// Used when the system can not rely on calibration data being present (e.g. during homing)
constexpr float ENCODER_MAGNET_PITCH = 3.0f; // [mm]
constexpr float ENCODER_MAGNET_RADIUS = 30.0f; // [mm]
constexpr float ENCODER_ANGLE_TO_ROTOR_ANGLE = (ENCODER_MAGNET_PITCH*2.0f) /
(ENCODER_MAGNET_RADIUS * Constants::TWO_PI_F);
//--- HOMING ------------------------------------------------------------------
constexpr float HOMING_VELOCITY = 1.0f; // rad per s
constexpr float HOMING_CURRENT = 0.15f; // range 0..1
constexpr float HOMING_FINISH_POS = 0.5f; // in rad
//--- CALIBRATION -------------------------------------------------------------
// degrees from home position
constexpr float CALIBRATION_RANGE = 95;
// velocity of the magnetic field during calibration (lower is more accurate)
constexpr float CALIBRATION_FIELD_VELOCITY = 40.0f;
//--- PINS --------------------------------------------------------------------
// #define SINGLE_AXIS_BOARD
#ifndef SINGLE_AXIS_BOARD
@ -75,24 +97,4 @@
#define PIN_ENCODER_SCK 2
#define PIN_ENCODER_MISO 0
#define PIN_ENCODER_MOSI 3
#endif
// test setup
/*
#define PIN_PWM_A_POS 2
#define PIN_PWM_A_NEG 3
#define PIN_PWM_B_POS 1
#define PIN_PWM_B_NEG 0
#define PIN_PWM_A_EN 5
#define PIN_PWM_B_EN 5
#define PIN_PWMAB 4
// I2C Encoder
//#define PIN_ENCODER_SDA 28
//#define PIN_ENCODER_SCL 29
#define PIN_ENCODER_CS 17
#define PIN_ENCODER_SCK 18
#define PIN_ENCODER_MISO 16
#define PIN_ENCODER_MOSI 19
*/
#endif

25
firmware/MotionControllerRP/src/robot.cpp
View file

@ -63,7 +63,8 @@ bool RobotJoint::calibrate(bool print_measurements) {
HomingController homing_controller;
bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY,
360.0f*DEG_TO_RAD, HOMING_CURRENT);
360.0f*DEG_TO_RAD, HOMING_CURRENT,
ENCODER_ANGLE_TO_ROTOR_ANGLE);
if(homing_ok == false) {
LOG_ERROR("Joint-%i: Calibration failed due to unsuccessful homing sequence", joint_idx);
return false;
@ -402,7 +403,7 @@ bool Robot::check_all_joints_ready() {
return all_ready;
}
bool Robot::home(uint8_t joint_mask) {
bool Robot::home(uint8_t joint_mask, float retract_angles[NUM_JOINTS]) {
HomingController homing_controller[NUM_JOINTS];
LOG_INFO("homing...");
enable_servo_control(false);
@ -416,7 +417,8 @@ bool Robot::home(uint8_t joint_mask) {
if(((joint_mask>>i)&1) == 0) continue;
LOG_DEBUG("start homing axis %i", i);
homing_controller[i].start(joints[i]->servo_controller,
-HOMING_VELOCITY, 360.0f*DEG_TO_RAD, HOMING_CURRENT);
-HOMING_VELOCITY, 360.0f*DEG_TO_RAD, HOMING_CURRENT,
ENCODER_ANGLE_TO_ROTOR_ANGLE, retract_angles[i]);
}
// run homing controllers
@ -760,17 +762,30 @@ void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::st
}
void Robot::process_home_command(const GCodeCommand& cmd, std::string& reply) {
float retract_angles[NUM_JOINTS] = {-1.0f};
// TODO: check parameter and build joint mask
uint8_t joint_mask = 0;
for(int i=0; i<NUM_JOINTS; i++) {
if(cmd.has_word('A'+i))
char word = 'A'+i;
if(cmd.has_word(word)) {
joint_mask |= 1<<i;
float retract_angle = cmd.get_value(word) * Constants::DEG2RAD;
if(retract_angle > 1e-3f)
retract_angles[i] = retract_angle;
}
}
std::string supported_words = "A,B,C,D,E,F";
if(cmd.contains_unsupported_words(supported_words+",G,M")) {
reply = "error: Unsupported parameter found. Only [" + supported_words + "] are supported\n";
return;
}
if(joint_mask == 0)
joint_mask = 255;
bool ok = home(joint_mask);
bool ok = home(joint_mask, retract_angles);
reply = ok ? "ok\n" : "error\n";
}

2
firmware/MotionControllerRP/src/robot.h
View file

@ -93,7 +93,7 @@ class Robot : public ICommandProcessor {
void init();
void calibrate();
bool home(uint8_t joint_mask=255);
bool home(uint8_t joint_mask, float retract_angles[NUM_JOINTS]);
bool calibrate_joint(int joint_idx, bool store_calibration, bool print_measurements);
void enable_servo_control(bool enable); // enables joint servo controll if homed and calibrated

44
firmware/MotionControllerRP/src/servo_control/homing_controller.cpp
View file

@ -4,28 +4,51 @@
#include "pico/time.h"
HomingController::HomingController() {
retract_field_velocity = 30.0f; // rad per second
retract_field_velocity = 100.0f; // rad per second
retract_field_angle = Constants::TWO_PI_F*0.25f;
}
bool HomingController::run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current) {
start(servo_controller, motor_velocity, search_range, current);
bool HomingController::run_blocking(ServoController* servo_controller,
float motor_velocity,
float search_range_angle,
float current,
float encoder_angle_to_motor_angle,
float retract_angle_rad)
{
start(servo_controller, motor_velocity, search_range_angle, current,
encoder_angle_to_motor_angle, retract_angle_rad);
while(is_finished() == false) {
update();
}
finalize();
return is_successful();
}
void HomingController::start(ServoController* servo_controller, float velocity, float range, float current) {
void HomingController::start(ServoController* servo_controller,
float velocity,
float search_range_angle,
float current,
float encoder_angle_to_motor_angle,
float retract_angle_rad)
{
float pole_pair_count = servo_controller->get_pole_pair_count();
float field_angle_to_encoder_angle = Constants::TWO_PI_F*30.0f/3.0f*0.5f / pole_pair_count;
float field_angle_to_encoder_angle = 1.0f / pole_pair_count / encoder_angle_to_motor_angle;
if(retract_angle_rad >= 0.0f)
HomingController::retract_field_angle = retract_angle_rad * pole_pair_count;
// field_angle_to_rotor_angle = 1.0 / pole_pair_count
// encoder_angle_to_rotor_angle = encoder_period_pitch/encoder_radius
// field_angle_to_encoder_angle = field_angle_to_rotor_angle/encoder_angle_to_rotor_angle
eval_field_angle_delta = Constants::TWO_PI_F*0.1f;
expected_encoder_delta = eval_field_angle_delta * field_angle_to_encoder_angle;
servo_ctrl = servo_controller;
field_velocity = velocity * pole_pair_count;
field_angle_search_range = range * pole_pair_count;
field_angle_search_range = search_range_angle * pole_pair_count;
homing_current = current;
auto& motor_driver = servo_ctrl->get_motor_driver();
@ -112,11 +135,14 @@ void HomingController::on_endstop_detected() {
void HomingController::finalize() {
auto& motor_driver = servo_ctrl->get_motor_driver();
float pole_pair_count = servo_ctrl->get_pole_pair_count();
// back off from home position
float backoff_field_angle = Constants::TWO_PI_F*0.25f;
motor_driver.rotate_field(backoff_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f),
retract_field_velocity, nullptr);
motor_driver.rotate_field(retract_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f),
retract_field_velocity, [this](){
// update encoder so it doesnt miss a period
servo_ctrl->get_encoder().read_abs_angle();
});
// restore previous motor current
motor_driver.set_amplitude_smooth(initial_current, 100);

27
firmware/MotionControllerRP/src/servo_control/homing_controller.h
View file

@ -25,10 +25,30 @@ class HomingController {
public:
HomingController();
// Starts the homing cycle, motor_velocity can be negative and defines the homing direction.
// Starts a blocking homing cycle, motor_velocity can be negative and defines the homing direction.
// WARNING: Servo loop updates (including encoder reads) must be completely disabled during homing.
bool run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current);
void start(ServoController* servo_controller, float motor_velocity, float search_range, float current);
// @param servo_controller: servo controller instance used for homing
// @param search_range_angle: search range angle for finding the endstop
// @param current: motor current factor used during homing, in range [0..1]
// @param encoder_angle_to_motor_angle: conversion factor from encoder angle to motor angle
// @param retract_angle_rad: retract angle after homing, default is used if negative values are provided
bool run_blocking(ServoController* servo_controller,
float motor_velocity,
float search_range_angle,
float current,
float encoder_angle_to_motor_angle,
float retract_angle_rad=-1.0f);
// Starts a non blocking homing cycle, motor_velocity can be negative and defines the homing direction.
// WARNING: Servo loop updates (including encoder reads) must be completely disabled during homing.
// Note: same parameter as 'run_blocking()'
void start(ServoController* servo_controller,
float motor_velocity,
float search_range,
float current,
float encoder_angle_to_motor_angle,
float retract_angle_rad=-1.0f);
void update();
void finalize();
@ -55,6 +75,7 @@ class HomingController {
float field_angle_search_range = 0.0f;
float homing_current = 0.0f;
float initial_current = 0.0f;
float retract_field_angle = 0.0f;
float retract_field_velocity = 0.0f;
// State machine

2
firmware/MotionControllerRP/src/version.h
View file

@ -1 +1 @@
static const char* FIRMWARE_VERSION = "v1.0.2";
static const char* FIRMWARE_VERSION = "v1.0.3";