* improved hw_config file

* added retract angle to homing
* v1.0.3

firmware/MotionControllerRP/src/command_parser/command_parser.cpp

@@ -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");

firmware/MotionControllerRP/src/command_parser/command_parser.h

@@ -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 -----------------------------------------------------------------

firmware/MotionControllerRP/src/hw_config.h

@@ -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
+constexpr float MOTOR1_POLE_PAIRS = 100;
-#define MOTOR2_POLE_PAIRS 100
+constexpr float MOTOR2_POLE_PAIRS = 100;
-#define MOTOR3_POLE_PAIRS 100
+constexpr float MOTOR3_POLE_PAIRS = 100;
 
-#define CALIBRATION_RANGE 95        // degrees from home position
+//--- ENCODERS ----------------------------------------------------------------
-#define CALIBRATION_FIELD_VELOCITY 40.0f
 
-#define HOMING_VELOCITY   1.0f        // rad per s
+// Conversion factor from encoder angle (one 2pi period every two magnets) to rotor angle.
-#define HOMING_CURRENT    0.15f       // range 0..1
+// Used when the system can not rely on calibration data being present (e.g. during homing)
-#define HOMING_FINISH_POS 0.5f        // in rad
+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
+#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
-*/

firmware/MotionControllerRP/src/robot.cpp

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

firmware/MotionControllerRP/src/robot.h

@@ -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
 

firmware/MotionControllerRP/src/servo_control/homing_controller.cpp

@@ -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) {
+bool HomingController::run_blocking(ServoController* servo_controller, 
-  start(servo_controller, motor_velocity, search_range, current);
+                                    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(retract_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f), 
-  motor_driver.rotate_field(backoff_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f), 
+                            retract_field_velocity, [this](){
-                            retract_field_velocity, nullptr);
+                              // 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);

firmware/MotionControllerRP/src/servo_control/homing_controller.h

@@ -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);
+    // @param servo_controller: servo controller instance used for homing
-    void start(ServoController* servo_controller, float motor_velocity, float search_range, float current);
+    // @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

firmware/MotionControllerRP/src/version.h

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