* improved hw_config file
* added retract angle to homing * v1.0.3
This commit is contained in:
0x23
parent
40d292ff46
commit
dc0278ea2b
8 changed files with 151 additions and 49 deletions
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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 -----------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
static const char* FIRMWARE_VERSION = "v1.0.2";
|
||||
static const char* FIRMWARE_VERSION = "v1.0.3";
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue