diff --git a/firmware/MotionControllerRP/src/command_parser/command_parser.cpp b/firmware/MotionControllerRP/src/command_parser/command_parser.cpp index 1410cd8..ab1a002 100644 --- a/firmware/MotionControllerRP/src/command_parser/command_parser.cpp +++ b/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= '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"); diff --git a/firmware/MotionControllerRP/src/command_parser/command_parser.h b/firmware/MotionControllerRP/src/command_parser/command_parser.h index c3c5edf..7eea46a 100644 --- a/firmware/MotionControllerRP/src/command_parser/command_parser.h +++ b/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 ----------------------------------------------------------------- diff --git a/firmware/MotionControllerRP/src/hw_config.h b/firmware/MotionControllerRP/src/hw_config.h index e380e33..6869428 100644 --- a/firmware/MotionControllerRP/src/hw_config.h +++ b/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 -#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 \ No newline at end of file diff --git a/firmware/MotionControllerRP/src/robot.cpp b/firmware/MotionControllerRP/src/robot.cpp index 2ec86df..7a113ad 100644 --- a/firmware/MotionControllerRP/src/robot.cpp +++ b/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 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"; } diff --git a/firmware/MotionControllerRP/src/robot.h b/firmware/MotionControllerRP/src/robot.h index bcb3e14..c2e917a 100644 --- a/firmware/MotionControllerRP/src/robot.h +++ b/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 diff --git a/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp b/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp index e3ecc7c..7b56c27 100644 --- a/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp +++ b/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) { - 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); diff --git a/firmware/MotionControllerRP/src/servo_control/homing_controller.h b/firmware/MotionControllerRP/src/servo_control/homing_controller.h index 32ae186..9df9d8e 100644 --- a/firmware/MotionControllerRP/src/servo_control/homing_controller.h +++ b/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); - 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 diff --git a/firmware/MotionControllerRP/src/version.h b/firmware/MotionControllerRP/src/version.h index e6baba0..754176c 100644 --- a/firmware/MotionControllerRP/src/version.h +++ b/firmware/MotionControllerRP/src/version.h @@ -1 +1 @@ -static const char* FIRMWARE_VERSION = "v1.0.2"; +static const char* FIRMWARE_VERSION = "v1.0.3";