Firmware Version 1.0.4
This commit is contained in:
0x23
parent
18313cc6ae
commit
e13cbda4cc
26 changed files with 794 additions and 256 deletions
|
|
@ -24,6 +24,38 @@ const std::string& GCodeCommand::get_command() const {
|
|||
return command;
|
||||
}
|
||||
|
||||
GCodeCommand::EParseStatus GCodeCommand::from_command_str(const char* gcode_command) {
|
||||
// Copy line into a mutable buffer
|
||||
char line_copy[256];
|
||||
strncpy(line_copy, gcode_command, sizeof(line_copy));
|
||||
line_copy[sizeof(line_copy) - 1] = '\0';
|
||||
|
||||
// Tokenize the first word (e.g., G0, M3, T1)
|
||||
char* saveptr = nullptr;
|
||||
char* token = strtok_r(line_copy, " ", &saveptr);
|
||||
if (!token || token[0] < 'A' || token[0] > 'Z') {
|
||||
return EParseStatus::MALFORMED_COMMAND;
|
||||
}
|
||||
|
||||
reset();
|
||||
set_command(token);
|
||||
|
||||
// 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')
|
||||
set_value(token[0], 0.0f);
|
||||
else
|
||||
set_value(token[0], strtof(token + 1, nullptr));
|
||||
} else {
|
||||
return EParseStatus::INVALID_PARAMETER;
|
||||
}
|
||||
}
|
||||
|
||||
return EParseStatus::OK;
|
||||
}
|
||||
|
||||
|
||||
void GCodeCommand::reset() {
|
||||
// Initialize all word values to NaN to represent "not set"
|
||||
for (float& value : word_values) {
|
||||
|
|
@ -116,6 +148,10 @@ void CommandParser::update() {
|
|||
}
|
||||
}
|
||||
|
||||
bool CommandParser::is_command_ready() {
|
||||
return command_ready;
|
||||
}
|
||||
|
||||
// Feed input chars one by one
|
||||
void CommandParser::add_input_character(char c) {
|
||||
if (c == '\n' || c == '\r') {
|
||||
|
|
@ -132,41 +168,19 @@ void CommandParser::add_input_character(char c) {
|
|||
bool CommandParser::parse_line(const char* line) {
|
||||
command_ready = false;
|
||||
|
||||
// Copy line into a mutable buffer
|
||||
char line_copy[256];
|
||||
strncpy(line_copy, line, sizeof(line_copy));
|
||||
line_copy[sizeof(line_copy) - 1] = '\0';
|
||||
// reset and parse gcode command from string
|
||||
GCodeCommand::EParseStatus ret = command.from_command_str(line);
|
||||
|
||||
// Tokenize the first word (e.g., G0, M3, T1)
|
||||
char* saveptr = nullptr;
|
||||
char* token = strtok_r(line_copy, " ", &saveptr);
|
||||
if (!token || token[0] < 'A' || token[0] > 'Z') {
|
||||
if(ret == GCodeCommand::EParseStatus::MALFORMED_COMMAND) {
|
||||
command_processor->send_reply("error: malformed command\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
command.reset();
|
||||
command.set_command(token);
|
||||
|
||||
// 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");
|
||||
return false;
|
||||
}
|
||||
if(ret == GCodeCommand::EParseStatus::INVALID_PARAMETER) {
|
||||
command_processor->send_reply("error: invalid parameter\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
command_ready = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns false if the command could not yet be processed.
|
||||
// The function will be called witht the same command later to try again.
|
||||
bool CommandParser::handle_gcode_command(const GCodeCommand& cmd) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
@ -16,8 +16,16 @@ static constexpr int LETTER_COUNT = 26;
|
|||
|
||||
class GCodeCommand {
|
||||
public:
|
||||
enum class EParseStatus {
|
||||
OK=1,
|
||||
MALFORMED_COMMAND=-1,
|
||||
INVALID_PARAMETER=-2
|
||||
};
|
||||
|
||||
GCodeCommand();
|
||||
|
||||
EParseStatus from_command_str(const char* cmd);
|
||||
|
||||
void reset();
|
||||
void set_command(const char* cmd);
|
||||
const std::string& get_command() const;
|
||||
|
|
@ -52,11 +60,9 @@ class CommandParser {
|
|||
|
||||
void set_command_processor(ICommandProcessor* cp);
|
||||
void add_input_character(char c); // Feed input chars one by one
|
||||
void update(); // Feed input chars one by one
|
||||
|
||||
protected:
|
||||
void update();
|
||||
bool is_command_ready(); // true if command was parsed and is waiting to be executed
|
||||
bool parse_line(const char* line);
|
||||
bool handle_gcode_command(const GCodeCommand& cmd);
|
||||
|
||||
private:
|
||||
char buffer[255];
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@
|
|||
#include "hardware/spi.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "utilities/logging.h"
|
||||
|
||||
void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz) {
|
||||
// Set GPIO functions to SPI
|
||||
|
|
@ -17,22 +18,27 @@ void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint
|
|||
// SPI format: 8 bits, mode 3 (CPOL=1, CPHA=1)
|
||||
spi_init(spi, baudrate_hz);
|
||||
spi_set_format(spi, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
|
||||
|
||||
sleep_ms(10);
|
||||
}
|
||||
|
||||
MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
|
||||
// nop
|
||||
if (cs_pin >= 0) {
|
||||
gpio_init(cs_pin);
|
||||
gpio_set_dir(cs_pin, GPIO_OUT);
|
||||
gpio_put(cs_pin, 1); // CS high
|
||||
}
|
||||
}
|
||||
|
||||
MT6835Encoder::~MT6835Encoder() {
|
||||
// nop
|
||||
}
|
||||
|
||||
void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
|
||||
if (cs_pin >= 0) {
|
||||
gpio_init(cs_pin);
|
||||
gpio_set_dir(cs_pin, GPIO_OUT);
|
||||
gpio_put(cs_pin, 1); // CS high
|
||||
}
|
||||
bool MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
|
||||
sleep_ms(100);
|
||||
|
||||
if(is_connected() == false)
|
||||
return false;
|
||||
|
||||
set_rotation_direction(0); // needs to be set, otherwise might be random
|
||||
set_bandwidth(bandwidth);
|
||||
|
|
@ -40,6 +46,25 @@ void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
|
|||
|
||||
last_raw_angle = 0;
|
||||
abs_raw_angle = 0;
|
||||
initialized = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::is_connected() {
|
||||
uint8_t check_bytes[] = {37, 109, 179, 251, 1};
|
||||
for(int i=0; i<sizeof(check_bytes); i++) {
|
||||
write_register(MT6835_REG_USERID, check_bytes[i]);
|
||||
uint8_t user_id = read_register(MT6835_REG_USERID);
|
||||
if(user_id != check_bytes[i])
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::is_initialized() {
|
||||
return initialized;
|
||||
}
|
||||
|
||||
void MT6835Encoder::reset_abs_angle(int32_t abs_raw_angle) {
|
||||
|
|
|
|||
|
|
@ -145,7 +145,10 @@ class MT6835Encoder {
|
|||
MT6835Encoder(spi_inst_t *spi, uint cs_pin);
|
||||
virtual ~MT6835Encoder();
|
||||
|
||||
void init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
|
||||
bool init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
|
||||
bool is_connected();
|
||||
bool is_initialized();
|
||||
|
||||
void reset_abs_angle(int32_t abs_raw_angle=0); // resets the total revolutions of abs angle
|
||||
void reset_abs_angle_period(); // Brings abs angle into [0..2pi)
|
||||
float read_abs_angle(); // returns the absolute angle in radians
|
||||
|
|
@ -198,6 +201,7 @@ class MT6835Encoder {
|
|||
bool check_crc = false;
|
||||
|
||||
private:
|
||||
bool initialized=false;
|
||||
spi_inst_t *spi;
|
||||
uint cs_pin;
|
||||
uint8_t last_status = 0;
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@ TB6612MotorDriver::TB6612MotorDriver(
|
|||
{
|
||||
max_pwm = (1 << pwm_resolution) - 1;
|
||||
set_amplitude(0.0f, false);
|
||||
field_angle = 0.0f;
|
||||
}
|
||||
|
||||
void init_output_pin(uint8_t pin, bool value) {
|
||||
|
|
@ -113,6 +114,9 @@ float TB6612MotorDriver::get_amplitude() const {
|
|||
}
|
||||
|
||||
void TB6612MotorDriver::rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step) {
|
||||
if(delta_angle == 0.0f)
|
||||
return;
|
||||
|
||||
float start = field_angle;
|
||||
float rad_per_µs = (delta_angle >= 0.0f) ? rad_per_s*1e-6f : -rad_per_s*1e-6f;
|
||||
|
||||
|
|
|
|||
111
firmware/MotionControllerRP/src/hardware/spi_interface.cpp
Normal file
111
firmware/MotionControllerRP/src/hardware/spi_interface.cpp
Normal file
|
|
@ -0,0 +1,111 @@
|
|||
#include "spi_interface.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
SpiDma::SpiDma(spi_inst_t* spi, uint sck, uint mosi, uint miso, uint cs)
|
||||
: spi(spi), sck(sck), mosi(mosi), miso(miso), cs(cs)
|
||||
{
|
||||
}
|
||||
|
||||
void SpiDma::begin(uint32_t baudrate_hz) {
|
||||
// SPI init
|
||||
spi_init(spi, baudrate_hz);
|
||||
spi_set_format(spi, 8,
|
||||
SPI_CPOL_0,
|
||||
SPI_CPHA_0,
|
||||
SPI_MSB_FIRST);
|
||||
|
||||
gpio_set_function(sck, GPIO_FUNC_SPI);
|
||||
gpio_set_function(mosi, GPIO_FUNC_SPI);
|
||||
gpio_set_function(miso, GPIO_FUNC_SPI);
|
||||
|
||||
// Manual CS
|
||||
gpio_init(cs);
|
||||
gpio_set_dir(cs, GPIO_OUT);
|
||||
gpio_put(cs, 1);
|
||||
|
||||
// DMA TX channel
|
||||
dma_tx = dma_claim_unused_channel(true);
|
||||
cfg_tx = dma_channel_get_default_config(dma_tx);
|
||||
channel_config_set_transfer_data_size(&cfg_tx, DMA_SIZE_8);
|
||||
channel_config_set_dreq(&cfg_tx, spi_get_dreq(spi, true));
|
||||
channel_config_set_read_increment(&cfg_tx, true);
|
||||
channel_config_set_write_increment(&cfg_tx, false);
|
||||
|
||||
// --- DMA RX channel ---
|
||||
dma_rx = dma_claim_unused_channel(true);
|
||||
cfg_rx = dma_channel_get_default_config(dma_rx);
|
||||
channel_config_set_transfer_data_size(&cfg_rx, DMA_SIZE_8);
|
||||
channel_config_set_dreq(&cfg_rx, spi_get_dreq(spi, false));
|
||||
channel_config_set_read_increment(&cfg_rx, false);
|
||||
channel_config_set_write_increment(&cfg_rx, true);
|
||||
|
||||
// Configure but don't start yet
|
||||
dma_channel_configure(
|
||||
dma_tx, &cfg_tx,
|
||||
&spi_get_hw(spi)->dr,
|
||||
nullptr, 0, false);
|
||||
|
||||
dma_channel_configure(
|
||||
dma_rx, &cfg_rx,
|
||||
nullptr,
|
||||
&spi_get_hw(spi)->dr,
|
||||
0, false);
|
||||
}
|
||||
|
||||
bool SpiDma::is_busy() const {
|
||||
return dma_channel_is_busy(dma_tx) ||
|
||||
dma_channel_is_busy(dma_rx);
|
||||
}
|
||||
|
||||
void SpiDma::transfer(const void* tx_buf, void* rx_buf, size_t bytes) {
|
||||
while (is_busy()) {}
|
||||
|
||||
gpio_put(cs, 0);
|
||||
|
||||
// Ensure minimum CS low time (50 ns) - Approx 14 cycles at 250 MHz
|
||||
for (int i = 0; i < 14; i++) __asm volatile("nop");
|
||||
|
||||
// Setup RX DMA - must be set first!
|
||||
if (rx_buf != nullptr) {
|
||||
dma_channel_set_write_addr(dma_rx, rx_buf, false);
|
||||
dma_channel_set_trans_count(dma_rx, bytes, false);
|
||||
}
|
||||
|
||||
// Setup TX DMA
|
||||
if (tx_buf != nullptr) {
|
||||
dma_channel_set_read_addr(dma_tx, tx_buf, false);
|
||||
dma_channel_set_trans_count(dma_tx, bytes, true);
|
||||
}
|
||||
}
|
||||
|
||||
void SpiDma::wait_for_finish() {
|
||||
while (is_busy()) {}
|
||||
gpio_put(cs, 1);
|
||||
|
||||
// Ensure minimum CS low time (50 ns) - Approx 14 cycles at 250 MHz
|
||||
for (int i = 0; i < 14; i++) __asm volatile("nop");
|
||||
}
|
||||
|
||||
void SpiDma::abort() {
|
||||
// Stop TX DMA if running
|
||||
if (dma_channel_is_busy(dma_tx)) {
|
||||
dma_channel_abort(dma_tx);
|
||||
}
|
||||
|
||||
// Stop RX DMA if running
|
||||
if (dma_channel_is_busy(dma_rx)) {
|
||||
dma_channel_abort(dma_rx);
|
||||
}
|
||||
|
||||
// Flush the SPI FIFOs to prevent leftover data
|
||||
spi_get_hw(spi)->dr = 0; // clear DR (TX FIFO)
|
||||
spi_get_hw(spi)->sr; // read SR to clear flags
|
||||
|
||||
// Deassert CS pin to leave SPI slave in idle state
|
||||
gpio_put(cs, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
37
firmware/MotionControllerRP/src/hardware/spi_interface.h
Normal file
37
firmware/MotionControllerRP/src/hardware/spi_interface.h
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
// --------------------------------------------------------------------------------------
|
||||
// Project: MicroManipulatorStepper
|
||||
// License: MIT (see LICENSE file for full description)
|
||||
// All text in here must be included in any redistribution.
|
||||
// Author: M. S. (diffraction limited)
|
||||
// --------------------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/dma.h"
|
||||
#include "hardware/gpio.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class SpiDma {
|
||||
public:
|
||||
SpiDma(spi_inst_t* spi, uint sck, uint mosi, uint miso, uint cs);
|
||||
|
||||
void begin(uint32_t baudrate_hz);
|
||||
bool is_busy() const;
|
||||
void transfer(const void* tx_buf, void* rx_buf, size_t bytes);
|
||||
void wait_for_finish();
|
||||
void abort();
|
||||
|
||||
private:
|
||||
spi_inst_t* spi;
|
||||
uint sck, mosi, miso, cs;
|
||||
|
||||
int dma_tx;
|
||||
int dma_rx;
|
||||
dma_channel_config cfg_tx;
|
||||
dma_channel_config cfg_rx;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,14 +1,20 @@
|
|||
#pragma once
|
||||
#include "utilities/math_constants.h"
|
||||
|
||||
// #define DEMO_MODE
|
||||
|
||||
//--- MOTORS ------------------------------------------------------------------
|
||||
|
||||
// motor pole pair count
|
||||
// * 100 for 0.9deg stepper motors
|
||||
// * 50 for 1.8deg stepper motors
|
||||
constexpr float MOTOR1_POLE_PAIRS = 100;
|
||||
constexpr float MOTOR2_POLE_PAIRS = 100;
|
||||
constexpr float MOTOR3_POLE_PAIRS = 100;
|
||||
constexpr float MOTOR1_POLE_PAIRS = 50;
|
||||
constexpr float MOTOR2_POLE_PAIRS = 50;
|
||||
constexpr float MOTOR3_POLE_PAIRS = 50;
|
||||
|
||||
// max current factor in range [0..1]. Lower values reduce pwm resolution so a
|
||||
// value above 0.4 is recommended.
|
||||
constexpr float MOTOR_MAX_CURRENT_FACTOR = 0.6f;
|
||||
|
||||
//--- ENCODERS ----------------------------------------------------------------
|
||||
|
||||
|
|
@ -28,18 +34,44 @@ constexpr float HOMING_FINISH_POS = 0.5f; // in rad
|
|||
//--- CALIBRATION -------------------------------------------------------------
|
||||
|
||||
// degrees from home position
|
||||
constexpr float CALIBRATION_RANGE = 95;
|
||||
constexpr float CALIBRATION_RANGE = 83;
|
||||
|
||||
// velocity of the magnetic field during calibration (lower is more accurate)
|
||||
constexpr float CALIBRATION_FIELD_VELOCITY = 40.0f;
|
||||
constexpr float CALIBRATION_FIELD_VELOCITY = 20.0f;
|
||||
|
||||
// size of the calibration lookup table
|
||||
constexpr int ENCODER_LUT_SIZE = 256;
|
||||
|
||||
//--- CLOSED LOOP CONTROL -----------------------------------------------------
|
||||
|
||||
// position controller
|
||||
constexpr float POS_KP = 60.0f;
|
||||
constexpr float POS_KI = 30000.0f;
|
||||
|
||||
// velocity controller
|
||||
constexpr float VEL_LOWPASS_TC = 0.004f;
|
||||
constexpr float VEL_KP = 0.2f;
|
||||
constexpr float VEL_KI = 90.0f;
|
||||
|
||||
//--- KINEMATIC ---------------------------------------------------------------
|
||||
|
||||
// Kinematic Parameters are defined kinematic_modes/kinematic_model_delta3d.cpp
|
||||
|
||||
// NUM_JOINTS and NUM_TOOLS are defined in 'path_segment.h'. Note that changing
|
||||
// the number of joints requires changing the kinematic model accordingly and
|
||||
// also requires the initialization of the correct number of 'RobotJoint' objects
|
||||
// in the Robtos init method.
|
||||
|
||||
//--- PINS --------------------------------------------------------------------
|
||||
|
||||
#define JOINT_READY_OVERRIDE
|
||||
|
||||
// #define SINGLE_AXIS_BOARD
|
||||
#ifndef SINGLE_AXIS_BOARD
|
||||
// Pins for 3Axis Board
|
||||
#define PIN_BUILTIN_LED 23
|
||||
#define PIN_USER_BUTTON 24
|
||||
// #define PIN_BUILTIN_LED 23 // RP2040 pico clone
|
||||
// #define PIN_USER_BUTTON 24 // RP2040 pico clone
|
||||
#define PIN_BUILTIN_LED 25
|
||||
|
||||
#define PIN_M1_PWM_A_POS 13
|
||||
#define PIN_M1_PWM_A_NEG 12
|
||||
|
|
@ -67,6 +99,9 @@ constexpr float CALIBRATION_FIELD_VELOCITY = 40.0f;
|
|||
#define PIN_ENCODER_MISO 0
|
||||
#define PIN_ENCODER_MOSI 3
|
||||
|
||||
#define PIN_TOOL1 16
|
||||
#define PIN_TOOL2 17
|
||||
|
||||
#else
|
||||
// Single Axis Board
|
||||
#define PIN_BUILTIN_LED 16
|
||||
|
|
|
|||
|
|
@ -14,11 +14,60 @@
|
|||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
#define HW_VERSION4 // remove this if you are using HW-v3 (Note geometry parameters might be slightly off for HW-v3.0!)
|
||||
|
||||
#ifdef HW_VERSION4
|
||||
|
||||
/**
|
||||
* Initializes the kinematic model and its geometric parameters for hardware version v4.0.
|
||||
*/
|
||||
KinematicModel_Delta3D::KinematicModel_Delta3D() {
|
||||
const float D2R = Constants::DEG2RAD;
|
||||
|
||||
// location of base origin in endeffector coordinate system (ee in neutral position)
|
||||
Vec3F base_offset(-47.5f, -47.5f, -47.5f);
|
||||
|
||||
// distance between the two spehere centers of the linkage rod (arms)
|
||||
arm_length = 36.25f*2;
|
||||
|
||||
// distance from arm attachment points to rotor axis
|
||||
rotor_radius = 15.0f;
|
||||
|
||||
// midpoint between the two attachment spheres on the endeffector in EE coordinate system
|
||||
ee_attachment_points[0] = Vec3F(3.54f, -11.5f, 4.5f);
|
||||
ee_attachment_points[1] = Vec3F(4.5f, 3.54f, -11.5f);
|
||||
ee_attachment_points[2] = Vec3F(-11.5f, 4.5f, 3.54f);
|
||||
|
||||
// set actuator transformations based on CAD model
|
||||
actuator_to_base[0].rotation = QuaternionF::from_axis_angle(Vec3F(0.0f, 0.0f, 1.0f), 90.0f*D2R);
|
||||
actuator_to_base[0].translation = Vec3F(-21.5f, 21.0f, 52.0f)+base_offset;
|
||||
|
||||
actuator_to_base[1].rotation = QuaternionF::from_axis_angle(Vec3F(1.0f, 0.0f, 1.0f), 180.0f*D2R);
|
||||
actuator_to_base[1].translation = Vec3F(52.0f, -21.5f, 21.0f)+base_offset;
|
||||
|
||||
actuator_to_base[2].rotation = QuaternionF::from_axis_angle(Vec3F(-1.0f, 0.0f, 0.0f), 90.0f*D2R);
|
||||
actuator_to_base[2].translation = Vec3F(21.0f, 52.0f, -21.5f)+base_offset;
|
||||
|
||||
// angle offset from home positionto center position of the rotor
|
||||
rotor_angle_offset[0] = 42.0f*Constants::DEG2RAD;
|
||||
rotor_angle_offset[1] = 42.0f*Constants::DEG2RAD;
|
||||
rotor_angle_offset[2] = 42.0f*Constants::DEG2RAD;
|
||||
|
||||
for(int i=0; i<3; i++)
|
||||
base_to_actuator[i] = actuator_to_base[i].inverse();
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
// Below is the old code for HW-v3.0 for compatibility, some values might be wrong and do not reflect the actual cad model.
|
||||
// If you use them please check them against your build. All these problems where fixed in HW-v4 and the new function above
|
||||
// uses the correct values.
|
||||
KinematicModel_Delta3D::KinematicModel_Delta3D() {
|
||||
const float D2R = Constants::DEG2RAD;
|
||||
|
||||
// offset to move base origin defined in CAD to endeffector origin near neutral position
|
||||
// real device
|
||||
|
||||
// REAL DEVICE <--- These might be slightly wrong
|
||||
Vec3F base_offset(-32.5f, -32.5f, -32.5f);
|
||||
arm_length = 73.8f;
|
||||
rotor_radius = 15.0f;
|
||||
|
|
@ -26,8 +75,7 @@ KinematicModel_Delta3D::KinematicModel_Delta3D() {
|
|||
ee_attachment_points[1] = Vec3F(2.0f, -0.5f, -14.5f);
|
||||
ee_attachment_points[2] = Vec3F(-14.5f, 2.0f, -0.5f);
|
||||
|
||||
// endeffector attachment points
|
||||
// CAD
|
||||
// CAD <--- These might be be more correct but not sure
|
||||
/*
|
||||
Vec3F base_offset(-30.5f, -30.5f, -30.5f);
|
||||
arm_length = 2*36.5;
|
||||
|
|
@ -55,6 +103,8 @@ KinematicModel_Delta3D::KinematicModel_Delta3D() {
|
|||
base_to_actuator[i] = actuator_to_base[i].inverse();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
int KinematicModel_Delta3D::get_joint_count() {
|
||||
return 3;
|
||||
}
|
||||
|
|
@ -65,7 +115,7 @@ bool KinematicModel_Delta3D::foreward(const float* joint_positions, Pose6DF& pos
|
|||
Vec3F p = arm_attachment_point(i, joint_positions[i]);
|
||||
p = actuator_to_base[i].transformPoint(p);
|
||||
|
||||
// apply ee attachment point offsets offset so that three sphere intersection can be used
|
||||
// apply ee attachment point offsets so that three sphere intersection can be used
|
||||
// to find ee position. This only works if there is no ee rotation.
|
||||
arm_attachment_points[i] = p-ee_attachment_points[i];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,7 +11,6 @@
|
|||
#include "hardware/pll.h"
|
||||
#include "hardware/vreg.h"
|
||||
|
||||
#include <NeoPixelConnect.h>
|
||||
#include <Wire.h>
|
||||
#include <algorithm>
|
||||
|
||||
|
|
@ -23,9 +22,11 @@
|
|||
#include "hw_config.h"
|
||||
#include "LittleFS.h"
|
||||
|
||||
#include "demo_gcode_generator.h"
|
||||
|
||||
//*** GLOBALS ***************************************************************************
|
||||
|
||||
NeoPixelConnect strip(PIN_BUILTIN_LED, 1);
|
||||
// NeoPixelConnect strip(PIN_BUILTIN_LED, 1);
|
||||
Robot robot(0.01f);
|
||||
|
||||
/*
|
||||
|
|
@ -68,16 +69,18 @@ void overclock() {
|
|||
}
|
||||
|
||||
void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
|
||||
strip.neoPixelSetValue(0, r, g, b, false);
|
||||
/* strip.neoPixelSetValue(0, r, g, b, false);
|
||||
delayMicroseconds(2000);
|
||||
strip.neoPixelShow();
|
||||
strip.neoPixelShow(); */
|
||||
}
|
||||
|
||||
void led_blink(uint8_t r, uint8_t g, uint8_t b, int count, int period_time_ms) {
|
||||
for(int i=0; i<count; i++) {
|
||||
set_led_color(r, g, b);
|
||||
gpio_put(PIN_BUILTIN_LED, 1);
|
||||
// set_led_color(r, g, b);
|
||||
sleep_ms(period_time_ms/2);
|
||||
set_led_color(0, 0, 0);
|
||||
// set_led_color(0, 0, 0);
|
||||
gpio_put(PIN_BUILTIN_LED, 0);
|
||||
sleep_ms(period_time_ms/2);
|
||||
}
|
||||
}
|
||||
|
|
@ -85,6 +88,11 @@ void led_blink(uint8_t r, uint8_t g, uint8_t b, int count, int period_time_ms) {
|
|||
void main_core0() {
|
||||
uint64_t last_time = time_us_64();
|
||||
|
||||
#ifdef DEMO_MODE
|
||||
auto* demo_gcode_generator = new DemoGcodeGenerator(&robot);
|
||||
demo_gcode_generator->run(); // blocks forever
|
||||
#endif
|
||||
|
||||
while(true) {
|
||||
// update motion controller
|
||||
robot.update_command_parser();
|
||||
|
|
@ -111,12 +119,17 @@ void main_core1() {
|
|||
}
|
||||
|
||||
void setup() {
|
||||
gpio_init(PIN_BUILTIN_LED);
|
||||
gpio_set_dir(PIN_BUILTIN_LED, GPIO_OUT);
|
||||
|
||||
led_blink(0, 0, 30, 3, 100);
|
||||
// stdio_init_all(); // Initializes USB or UART stdio
|
||||
overclock();
|
||||
// Serial.begin(921600);
|
||||
Logger::instance().begin(921600, false);
|
||||
#ifndef DEMO_MODE
|
||||
while(!Serial);
|
||||
#endif
|
||||
|
||||
set_led_color(50, 10, 0);
|
||||
// auto* test = new KinematicModel_Delta3D(); test->test(); delete test;
|
||||
|
|
@ -146,6 +159,8 @@ void setup() {
|
|||
LOG_INFO("Initialization finished");
|
||||
LOG_INFO(" ");
|
||||
|
||||
gpio_put(PIN_BUILTIN_LED, 1);
|
||||
|
||||
return;
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -18,7 +18,10 @@ MotionController::MotionController(PathPlanner* path_planner) {
|
|||
current_time = 0.0f;
|
||||
}
|
||||
|
||||
bool MotionController::update(float dt, float* joint_positions, float* joint_velocities) {
|
||||
bool MotionController::update(float dt,
|
||||
float* joint_positions,
|
||||
float* joint_velocities,
|
||||
float* tool_outputs) {
|
||||
// increment time counter
|
||||
current_time += dt;
|
||||
|
||||
|
|
@ -49,6 +52,6 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
|
|||
current_path_segment.initialized = false;
|
||||
|
||||
// evaluate path segment
|
||||
current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
|
||||
current_path_segment.evaluate(current_time, joint_positions, joint_velocities, tool_outputs);
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ class MotionController {
|
|||
|
||||
// updates the motion controller and computes new joint positions and velocities
|
||||
// after dt has passed. Ouput array must hav space for 'NUM_JOINTS' entries.
|
||||
bool update(float dt, float* joint_positions, float* joint_velocities);
|
||||
bool update(float dt, float* joint_positions, float* joint_velocities, float* tool_outputs);
|
||||
|
||||
private:
|
||||
PathPlanner* path_planner;
|
||||
|
|
|
|||
|
|
@ -112,12 +112,15 @@ float MotionProfileConstAcc::evaluate(float time) const {
|
|||
|
||||
CartesianPathSegment::CartesianPathSegment() {
|
||||
dwell_time = 0.0f;
|
||||
for(int i=0; i<NUM_TOOLS; i++)
|
||||
tool_outputs[i] = 0.0f;
|
||||
}
|
||||
|
||||
CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
|
||||
const Pose6DF& end_pose,
|
||||
const LinearAngular& target_velocity,
|
||||
const LinearAngular& max_acceleration)
|
||||
const LinearAngular& max_acceleration,
|
||||
const float tool_outputs[NUM_TOOLS])
|
||||
{
|
||||
CartesianPathSegment::dwell_time = 0.0f;
|
||||
CartesianPathSegment::start_pose = start_pose;
|
||||
|
|
@ -134,6 +137,8 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
|
|||
|
||||
translation_delta_normalized = translation_delta.normalized();
|
||||
|
||||
for(int i=0; i<NUM_TOOLS; i++)
|
||||
CartesianPathSegment::tool_outputs[i] = tool_outputs[i];
|
||||
|
||||
/*
|
||||
QuaternionF rotation_delta = (end_pose.rotation * start_pose.rotation.normalized_inverse());
|
||||
|
|
@ -143,7 +148,9 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
|
|||
rotation_delta_axis = axis; */
|
||||
}
|
||||
|
||||
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
|
||||
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose,
|
||||
const float tool_outputs[NUM_TOOLS],
|
||||
float dwell_time)
|
||||
{
|
||||
CartesianPathSegment::dwell_time = dwell_time;
|
||||
CartesianPathSegment::start_pose = pose;
|
||||
|
|
@ -156,6 +163,9 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time
|
|||
|
||||
travel_distance.linear = 0.0f;
|
||||
travel_distance.angular = 0.0f;
|
||||
|
||||
for(int i=0; i<NUM_TOOLS; i++)
|
||||
CartesianPathSegment::tool_outputs[i] = tool_outputs[i];
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -209,6 +219,7 @@ JointSpacePathSegment::JointSpacePathSegment() {
|
|||
JointSpacePathSegment::JointSpacePathSegment(
|
||||
const float start_pos[NUM_JOINTS],
|
||||
const float end_pos[NUM_JOINTS],
|
||||
const float tool_outputs[NUM_TOOLS],
|
||||
float duration)
|
||||
{
|
||||
JointSpacePathSegment::duration = duration;
|
||||
|
|
@ -219,20 +230,31 @@ JointSpacePathSegment::JointSpacePathSegment(
|
|||
JointSpacePathSegment::end_pos[i] = end_pos[i];
|
||||
}
|
||||
|
||||
for(int i=0; i<NUM_TOOLS; i++) {
|
||||
JointSpacePathSegment::tool_outputs[i] = tool_outputs[i];
|
||||
}
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
void JointSpacePathSegment::evaluate(
|
||||
float time,
|
||||
float joint_positions[NUM_JOINTS],
|
||||
float joint_velocity[NUM_JOINTS]) const
|
||||
float joint_velocity[NUM_JOINTS],
|
||||
float tool_outputs[NUM_TOOLS]) const
|
||||
{
|
||||
float t = time*inv_duration;
|
||||
float s = 1.0f-t;
|
||||
|
||||
// perform linear interpolation
|
||||
for(int i=0; i<NUM_JOINTS; i++) {
|
||||
joint_positions[i] = start_pos[i]*s + end_pos[i]*t;
|
||||
joint_velocity[i] = 0;
|
||||
joint_velocity[i] = 0; // velocity currently not computed (TODO)
|
||||
}
|
||||
|
||||
// copy tool putput (tool outputs are not interpolated)
|
||||
for(int i=0; i<NUM_TOOLS; i++) {
|
||||
tool_outputs[i] = JointSpacePathSegment::tool_outputs[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -294,7 +316,10 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
|
|||
|
||||
// create joint space path segment
|
||||
float duration = current_time-initial_time;
|
||||
js_path_segment = JointSpacePathSegment(current_joint_pos, next_joint_pos, duration);
|
||||
js_path_segment = JointSpacePathSegment(current_joint_pos,
|
||||
next_joint_pos,
|
||||
path_segment->tool_outputs,
|
||||
duration);
|
||||
|
||||
// update current joint pos
|
||||
for(int i=0; i<NUM_JOINTS; i++)
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
//*** CONST *****************************************************************************
|
||||
|
||||
constexpr int NUM_JOINTS = 3;
|
||||
constexpr int NUM_TOOLS = 2;
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
|
|
@ -63,11 +64,16 @@ class MotionProfileConstAcc {
|
|||
class CartesianPathSegment {
|
||||
public:
|
||||
CartesianPathSegment();
|
||||
|
||||
CartesianPathSegment(const Pose6DF& start_pose,
|
||||
const Pose6DF& end_pose,
|
||||
const LinearAngular& velocity,
|
||||
const LinearAngular& max_acceleration);
|
||||
CartesianPathSegment(const Pose6DF& pose,float dwell_time);
|
||||
const LinearAngular& max_acceleration,
|
||||
const float tool_outputs[NUM_TOOLS]);
|
||||
|
||||
CartesianPathSegment(const Pose6DF& pose,
|
||||
const float tool_outputs[NUM_TOOLS],
|
||||
float dwell_time);
|
||||
|
||||
void evaluate(float time, Pose6DF& pose) const;
|
||||
float get_duration() const;
|
||||
|
|
@ -90,6 +96,7 @@ class CartesianPathSegment {
|
|||
MotionProfileConstAcc motion_profile;
|
||||
|
||||
float dwell_time; // stay at start position for given duration if dwell_time > 0
|
||||
float tool_outputs[NUM_TOOLS];
|
||||
};
|
||||
|
||||
//--- JointSpacePathSegment -------------------------------------------------------------
|
||||
|
|
@ -100,11 +107,13 @@ class JointSpacePathSegment {
|
|||
JointSpacePathSegment();
|
||||
JointSpacePathSegment(const float start_pos[NUM_JOINTS],
|
||||
const float end_pos[NUM_JOINTS],
|
||||
const float tool_outputs[NUM_TOOLS],
|
||||
const float duration);
|
||||
|
||||
void evaluate(float time,
|
||||
float joint_positions[NUM_JOINTS],
|
||||
float joint_velocity[NUM_JOINTS]) const;
|
||||
float joint_velocity[NUM_JOINTS],
|
||||
float tool_outputs[NUM_TOOLS]) const;
|
||||
|
||||
float get_duration();
|
||||
|
||||
|
|
@ -116,6 +125,7 @@ class JointSpacePathSegment {
|
|||
float end_pos[NUM_JOINTS];
|
||||
float start_velocity[NUM_JOINTS];
|
||||
float end_velocity[NUM_JOINTS];
|
||||
float tool_outputs[NUM_TOOLS];
|
||||
|
||||
float duration;
|
||||
float inv_duration;
|
||||
|
|
|
|||
|
|
@ -13,12 +13,17 @@
|
|||
#include "kinematic_models/kinematic_model_delta3d.h"
|
||||
#include "servo_control/homing_controller.h"
|
||||
#include "servo_control/actuator_calibration.h"
|
||||
#include "robot_joint/robot_joint.h"
|
||||
#include "pico/multicore.h"
|
||||
#include "version.h"
|
||||
#include "robot_tool/pwm_tool.h"
|
||||
|
||||
constexpr int SPINLOCK_ID_SHARED_DATA = 0;
|
||||
constexpr int SPINLOCK_ID_JOINTS = 1;
|
||||
|
||||
#include <NeoPixelConnect.h>
|
||||
NeoPixelConnect led(PIN_BUILTIN_LED, 1);
|
||||
|
||||
//*** FUNCTION **************************************************************************
|
||||
|
||||
bool startswith(const std::string& str, const std::string& prefix) {
|
||||
|
|
@ -28,123 +33,6 @@ bool startswith(const std::string& str, const std::string& prefix) {
|
|||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
//--- RobotAxis -------------------------------------------------------------------------
|
||||
|
||||
RobotJoint::RobotJoint(MT6835Encoder* encoder,
|
||||
TB6612MotorDriver* motor_driver,
|
||||
int pole_pairs)
|
||||
{
|
||||
RobotJoint::encoder = encoder;
|
||||
RobotJoint::motor_driver = motor_driver;
|
||||
servo_controller = new ServoController(*motor_driver, *encoder, pole_pairs);
|
||||
position = 0.0f;
|
||||
velocity = 0.0f;
|
||||
}
|
||||
|
||||
RobotJoint::~RobotJoint() {
|
||||
delete servo_controller;
|
||||
delete motor_driver;
|
||||
delete encoder;
|
||||
servo_controller = nullptr;
|
||||
motor_driver = nullptr;
|
||||
encoder = nullptr;
|
||||
}
|
||||
|
||||
void RobotJoint::init(int joint_idx) {
|
||||
RobotJoint::joint_idx = joint_idx;
|
||||
|
||||
encoder->init(0x5, 0x4);
|
||||
servo_controller->init(0.5);
|
||||
servo_controller->set_motor_enabled(false, false);
|
||||
}
|
||||
|
||||
bool RobotJoint::calibrate(bool print_measurements) {
|
||||
LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
|
||||
|
||||
HomingController homing_controller;
|
||||
bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY,
|
||||
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;
|
||||
}
|
||||
|
||||
// measure lookup tables
|
||||
LookupTable encoder_raw_to_motor_pos_lut;
|
||||
LookupTable motor_pos_to_field_angle_lut;
|
||||
bool ok = measure_calibration_data(encoder_raw_to_motor_pos_lut,
|
||||
motor_pos_to_field_angle_lut,
|
||||
*servo_controller,
|
||||
CALIBRATION_RANGE*DEG_TO_RAD,
|
||||
CALIBRATION_FIELD_VELOCITY,
|
||||
256,
|
||||
print_measurements);
|
||||
if(!ok) {
|
||||
LOG_ERROR("Joint-%i: calibrating failed", joint_idx);
|
||||
return false;
|
||||
}
|
||||
|
||||
servo_controller->set_enc_to_pos_lut(encoder_raw_to_motor_pos_lut);
|
||||
servo_controller->set_pos_to_field_lut(motor_pos_to_field_angle_lut);
|
||||
is_calibrated = true;
|
||||
is_homed = true;
|
||||
|
||||
LOG_INFO("Joint-%i: calibrating joint successful.", joint_idx);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void RobotJoint::update(float dt, float one_over_dt) {
|
||||
servo_controller->update(position, dt, one_over_dt);
|
||||
}
|
||||
|
||||
void RobotJoint::update_target(float p, float v) {
|
||||
position = p;
|
||||
velocity = v;
|
||||
}
|
||||
|
||||
bool RobotJoint::load_calibration() {
|
||||
std::string fn1 = calib_data_filename("enc_to_pos_lut").c_str();
|
||||
std::string fn2 = calib_data_filename("pos_to_field_lut").c_str();
|
||||
if(!LittleFS.exists(fn1.c_str()) || !LittleFS.exists(fn2.c_str())) {
|
||||
LOG_WARNING("Joint-%i: Not all calibration files found. Run joint calibration with M56.", joint_idx);
|
||||
return false;
|
||||
}
|
||||
|
||||
LookupTable enc_to_pos_lut;
|
||||
LookupTable pos_to_field_lut;
|
||||
bool res = true;
|
||||
res &= load_lut_from_file(enc_to_pos_lut, fn1.c_str());
|
||||
res &= load_lut_from_file(pos_to_field_lut, fn2.c_str());
|
||||
if(res == false)
|
||||
return false;
|
||||
|
||||
servo_controller->set_enc_to_pos_lut(enc_to_pos_lut);
|
||||
servo_controller->set_pos_to_field_lut(pos_to_field_lut);
|
||||
|
||||
is_calibrated = true;
|
||||
LOG_INFO("Joint-%i: Encoder lookup tables loaded (size=%i,%i)",
|
||||
joint_idx, enc_to_pos_lut.size(), pos_to_field_lut.size());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RobotJoint::store_calibration() {
|
||||
bool res = true;
|
||||
|
||||
res &= save_lut_to_file(servo_controller->get_enc_to_pos_lut(),
|
||||
calib_data_filename("enc_to_pos_lut").c_str());
|
||||
res &= save_lut_to_file(servo_controller->get_pos_to_field_lut(),
|
||||
calib_data_filename("pos_to_field_lut").c_str());
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
std::string RobotJoint::calib_data_filename(std::string data_name) const {
|
||||
return std::string("joint")+std::to_string(joint_idx)+"_"+data_name+".dat";
|
||||
}
|
||||
|
||||
//--- Robot -----------------------------------------------------------------------------
|
||||
|
||||
Robot::Robot(float path_segment_time_step) :
|
||||
|
|
@ -167,6 +55,9 @@ Robot::Robot(float path_segment_time_step) :
|
|||
max_acceleration = LinearAngular(500.0f, 50.0f);
|
||||
path_buffering_time_us = 50*1e3;
|
||||
|
||||
for(int i=0; i<NUM_TOOLS; i++)
|
||||
robot_tools[i] = nullptr;
|
||||
|
||||
state = ERobotState::IDLE;
|
||||
}
|
||||
|
||||
|
|
@ -220,6 +111,13 @@ void Robot::init() {
|
|||
joints[i]->load_calibration();
|
||||
}
|
||||
|
||||
// add tools
|
||||
robot_tools[0] = new PwmTool();
|
||||
((PwmTool*)robot_tools[0])->init(PIN_TOOL1, 8000, 8);
|
||||
|
||||
robot_tools[1] = new PwmTool();
|
||||
((PwmTool*)robot_tools[1])->init(PIN_TOOL2, 8000, 8);
|
||||
|
||||
// setup timer for updating the motion controller (which evaluates joint space path
|
||||
// segments and produces the current target position for the servo loops)
|
||||
float motion_controller_update_time_us = 500;
|
||||
|
|
@ -280,13 +178,14 @@ void Robot::update_path_planner() {
|
|||
}
|
||||
|
||||
/**
|
||||
* Updates the motion controller with a timer interrupt in regular intervals.
|
||||
* Updates the motion controller with a timer interrupt in regular intervals (e.g. 2kHz).
|
||||
* The function evaluates joint space path segments and produces the current
|
||||
* target position for the servo loops.
|
||||
*/
|
||||
bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
|
||||
float joint_positions[NUM_JOINTS];
|
||||
float joint_velocities[NUM_JOINTS];
|
||||
float tool_outputs[NUM_TOOLS];
|
||||
|
||||
// get robot pointer
|
||||
Robot* robot = (Robot*)timer->user_data;
|
||||
|
|
@ -296,8 +195,8 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
|
|||
float dt = float(time_us - robot->last_mc_update_time)*1e-6f;
|
||||
robot->last_mc_update_time = time_us;
|
||||
|
||||
// get current joint position/velocity
|
||||
bool update_ok = robot->motion_controller.update(dt, joint_positions, joint_velocities);
|
||||
// get current joint position/velocity and tool outputs
|
||||
bool update_ok = robot->motion_controller.update(dt, joint_positions, joint_velocities, tool_outputs);
|
||||
|
||||
// Attempt to acquire spinlock non-blocking and set new target data for the servo loops
|
||||
if (update_ok && spin_try_lock_unsafe(robot->shared_data.lock)) {
|
||||
|
|
@ -308,6 +207,16 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
|
|||
spin_unlock_unsafe(robot->shared_data.lock);
|
||||
}
|
||||
|
||||
// update tool outputs
|
||||
if(update_ok) {
|
||||
auto& tools = robot->robot_tools;
|
||||
for(int i=0; i<tools.size(); i++) {
|
||||
auto* tool = tools[i];
|
||||
if(tool != nullptr)
|
||||
tool->set_value(tool_outputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// update frequency counter
|
||||
robot->motion_controller_frequency_counter.update(dt);
|
||||
|
||||
|
|
@ -315,7 +224,7 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
|
|||
}
|
||||
|
||||
/**
|
||||
* update servo loops, this is called from a second cpu core
|
||||
* update servo loops, this is called from the second cpu core
|
||||
*/
|
||||
void Robot::update_servo_controllers(float dt) {
|
||||
float one_over_dt = 1.0f/dt;
|
||||
|
|
@ -394,6 +303,10 @@ Pose6DF Robot::pose_from_joint_angles() {
|
|||
return pose;
|
||||
}
|
||||
|
||||
CommandParser* Robot::get_command_parser() {
|
||||
return &command_parser;
|
||||
}
|
||||
|
||||
bool Robot::check_all_joints_ready() {
|
||||
bool all_ready = true;
|
||||
for(int i=0; i<NUM_JOINTS; i++) {
|
||||
|
|
@ -537,6 +450,12 @@ void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
|
|||
void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
reply = "";
|
||||
|
||||
// process tool output command
|
||||
if(cmd.get_command() == "M3") {
|
||||
process_tool_output_command(cmd, reply);
|
||||
return;
|
||||
}
|
||||
|
||||
// enable motors
|
||||
if(cmd.get_command() == "M17") {
|
||||
// read current pose from HW and set it as current pose
|
||||
|
|
@ -550,6 +469,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
spin_unlock_unsafe(joints_spin_lock);
|
||||
|
||||
reply = "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// disable motors
|
||||
|
|
@ -560,6 +480,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
spin_unlock_unsafe(joints_spin_lock);
|
||||
|
||||
reply = "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// get current internal position (not using encoders to read physical position)
|
||||
|
|
@ -568,6 +489,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
reply += std::string(" Y") + std::to_string(current_pose.translation.y);
|
||||
reply += std::string(" Z") + std::to_string(current_pose.translation.z);
|
||||
reply += "\nok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// get current internal position (not using encoders to read physical position)
|
||||
|
|
@ -579,6 +501,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
std::to_string(angle) + " deg (raw="+std::to_string(raw_angle)+")\n";
|
||||
}
|
||||
reply += "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// get planner queue size
|
||||
|
|
@ -586,6 +509,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
int s = path_planner.input_queue_size();
|
||||
reply += std::string("Queue Size: ") + std::to_string(s) + "\n";
|
||||
reply += "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// check if all planned motions are finished executing
|
||||
|
|
@ -593,16 +517,19 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
bool f = path_planner.all_finished();
|
||||
reply += f ? "1\n" : "0\n";
|
||||
reply += "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// set servo loop parameters
|
||||
if(cmd.get_command() == "M55") {
|
||||
process_set_servo_parameter_command(cmd, reply);
|
||||
return;
|
||||
}
|
||||
|
||||
// calibrate joint
|
||||
if(cmd.get_command() == "M56") {
|
||||
process_calibrate_joint_command(cmd, reply);
|
||||
return;
|
||||
}
|
||||
|
||||
// get info
|
||||
|
|
@ -624,23 +551,29 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
reply += std::string("Servo Loop: ") + std::to_string(servo_loop_freq/1000) + " kHz\n";
|
||||
reply += std::string("Motion Controler: ") + std::to_string(mcontroler_freq) + " Hz\n";
|
||||
|
||||
for(int i=0; i<NUM_TOOLS; i++)
|
||||
reply += std::string("Tool[") + std::to_string(i) + "] output: " + std::to_string(current_tool_outputs[i]) + "\n";
|
||||
|
||||
// file list
|
||||
reply += std::string("Files on flash: \n");
|
||||
auto file_list = get_file_list("/", true);
|
||||
for(auto& f : file_list) reply += std::string(" ")+f+"\n";
|
||||
reply += "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// get firmware version
|
||||
if(cmd.get_command() == "M58") {
|
||||
reply = std::string(FIRMWARE_VERSION)+"\n";
|
||||
reply += "ok\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// print lookup table
|
||||
if(cmd.get_command() == "M59") {
|
||||
int idx = (int)cmd.get_value('J', 0);
|
||||
joints[idx]->servo_controller->get_enc_to_pos_lut().print_to_log();
|
||||
return;
|
||||
}
|
||||
|
||||
// set linear and angular acceleration
|
||||
|
|
@ -648,16 +581,20 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
|
|||
if(cmd.has_word('L')) max_acceleration.linear = cmd.get_value('L');
|
||||
if(cmd.has_word('A')) max_acceleration.angular = cmd.get_value('A');
|
||||
reply += "ok\n";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
Pose6DF end_pose;
|
||||
|
||||
#ifndef JOINT_READY_OVERRIDE
|
||||
if(!all_joints_ready) {
|
||||
reply = "error: not all joints calibrated and homed\n";
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
if(path_planner.input_queue_full()) {
|
||||
reply = "busy\n";
|
||||
return;
|
||||
|
|
@ -684,9 +621,11 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
|
|||
}
|
||||
|
||||
// create path segment
|
||||
CartesianPathSegment path_segment(current_pose, end_pose,
|
||||
CartesianPathSegment path_segment(current_pose,
|
||||
end_pose,
|
||||
current_feedrate,
|
||||
max_acceleration);
|
||||
max_acceleration,
|
||||
current_tool_outputs);
|
||||
|
||||
bool ok = path_planner.add_cartesian_path_segment(path_segment);
|
||||
if(ok) {
|
||||
|
|
@ -698,6 +637,10 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
|
|||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Immediately sets the current pose without creating path segments or
|
||||
* interpolating from current position. Useful for external realtime controll.
|
||||
*/
|
||||
void Robot::process_set_pose_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
Pose6DF pose;
|
||||
|
||||
|
|
@ -725,10 +668,17 @@ void Robot::process_set_pose_command(const GCodeCommand& cmd, std::string& reply
|
|||
}
|
||||
|
||||
void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
#ifndef JOINT_READY_OVERRIDE
|
||||
if(!all_joints_ready) {
|
||||
reply = "error: not all joints calibrated and homed\n";
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
if(path_planner.input_queue_full()) {
|
||||
reply = "busy\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// get dwell time
|
||||
float dwell_time = 1.0f;
|
||||
|
|
@ -736,11 +686,15 @@ void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
|
|||
if(cmd.has_word('P')) dwell_time = cmd.get_value('P')*0.001f; // time given in milliseconds
|
||||
|
||||
// create path segment
|
||||
CartesianPathSegment path_segment(current_pose, dwell_time);
|
||||
path_planner.add_cartesian_path_segment(path_segment);
|
||||
path_planner.run_look_ahead_planning();
|
||||
CartesianPathSegment path_segment(current_pose, current_tool_outputs, dwell_time);
|
||||
bool ok = path_planner.add_cartesian_path_segment(path_segment);
|
||||
|
||||
reply = "ok\n";
|
||||
if(ok) {
|
||||
path_planner.run_look_ahead_planning();
|
||||
reply = "ok\n";
|
||||
} else {
|
||||
reply = "error\n";
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
|
|
@ -798,3 +752,19 @@ void Robot::process_calibrate_joint_command(const GCodeCommand& cmd, std::string
|
|||
bool ok = calibrate_joint(idx, store_calibration, print_measurements);
|
||||
reply = ok ? "ok\n" : "error\n";
|
||||
}
|
||||
|
||||
void Robot::process_tool_output_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
// get tool index
|
||||
int tool_index = (int)cmd.get_value('T', 0);
|
||||
if(tool_index < 0 || tool_index >= NUM_TOOLS) {
|
||||
reply = "error: Tool index out of range\n";
|
||||
return;
|
||||
}
|
||||
|
||||
// set current tool output value
|
||||
float tool_value = cmd.get_value('S', 0.0f);
|
||||
current_tool_outputs[tool_index] = tool_value;
|
||||
|
||||
reply = "ok\n";
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,17 +20,11 @@
|
|||
#include "motion_control/motion_controller.h"
|
||||
#include "command_parser/command_parser.h"
|
||||
|
||||
constexpr int ENCODER_LUT_SIZE = 256;
|
||||
|
||||
//*** CALSS *****************************************************************************
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class Robot;
|
||||
|
||||
//--- PersistentData --------------------------------------------------------------------
|
||||
|
||||
struct PersistentRobotData {
|
||||
float encoder_lut[NUM_JOINTS][ENCODER_LUT_SIZE];
|
||||
};
|
||||
class RobotJoint;
|
||||
class IRobotTool;
|
||||
|
||||
//--- SharedData ------------------------------------------------------------------------
|
||||
|
||||
|
|
@ -54,36 +48,6 @@ struct SharedData {
|
|||
spin_lock_t* lock = nullptr;
|
||||
};
|
||||
|
||||
//--- RobotJoint ------------------------------------------------------------------------
|
||||
|
||||
class RobotJoint {
|
||||
public:
|
||||
RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
|
||||
~RobotJoint();
|
||||
|
||||
void init(int joint_idx);
|
||||
bool calibrate(bool print_measurements);
|
||||
void update(float dt, float one_over_dt);
|
||||
void update_target(float p, float v);
|
||||
bool load_calibration();
|
||||
bool store_calibration();
|
||||
|
||||
private:
|
||||
std::string calib_data_filename(std::string data_name) const;
|
||||
|
||||
public:
|
||||
int joint_idx = 0;
|
||||
bool is_homed = false;
|
||||
bool is_calibrated = false;
|
||||
|
||||
float position;
|
||||
float velocity;
|
||||
|
||||
MT6835Encoder* encoder;
|
||||
TB6612MotorDriver* motor_driver;
|
||||
ServoController* servo_controller;
|
||||
};
|
||||
|
||||
//--- Robot -----------------------------------------------------------------------------
|
||||
|
||||
class Robot : public ICommandProcessor {
|
||||
|
|
@ -103,6 +67,8 @@ class Robot : public ICommandProcessor {
|
|||
|
||||
void set_pose(const Pose6DF& pos);
|
||||
Pose6DF pose_from_joint_angles();
|
||||
|
||||
CommandParser* get_command_parser();
|
||||
|
||||
public:
|
||||
void send_reply(const char* str) override;
|
||||
|
|
@ -116,6 +82,7 @@ class Robot : public ICommandProcessor {
|
|||
void process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply);
|
||||
void process_home_command(const GCodeCommand& cmd, std::string& reply);
|
||||
void process_calibrate_joint_command(const GCodeCommand& cmd, std::string& reply);
|
||||
void process_tool_output_command(const GCodeCommand& cmd, std::string& reply);
|
||||
|
||||
protected:
|
||||
bool check_all_joints_ready(); // checks if all joints are homed and calibrated
|
||||
|
|
@ -136,6 +103,7 @@ class Robot : public ICommandProcessor {
|
|||
CommandParser command_parser;
|
||||
|
||||
Pose6DF current_pose;
|
||||
float current_tool_outputs[NUM_TOOLS];
|
||||
LinearAngular max_acceleration;
|
||||
LinearAngular current_feedrate;
|
||||
|
||||
|
|
@ -146,4 +114,6 @@ class Robot : public ICommandProcessor {
|
|||
|
||||
FrequencyCounter servo_loop_frequency_counter;
|
||||
FrequencyCounter motion_controller_frequency_counter;
|
||||
|
||||
std::array<IRobotTool*, NUM_TOOLS> robot_tools;
|
||||
};
|
||||
|
|
|
|||
139
firmware/MotionControllerRP/src/robot_joint/robot_joint.cpp
Normal file
139
firmware/MotionControllerRP/src/robot_joint/robot_joint.cpp
Normal file
|
|
@ -0,0 +1,139 @@
|
|||
// --------------------------------------------------------------------------------------
|
||||
// Project: MicroManipulatorStepper
|
||||
// License: MIT (see LICENSE file for full description)
|
||||
// All text in here must be included in any redistribution.
|
||||
// Author: M. S. (diffraction limited)
|
||||
// --------------------------------------------------------------------------------------
|
||||
|
||||
#include <LittleFS.h>
|
||||
#include "robot_joint.h"
|
||||
#include "hw_config.h"
|
||||
#include "utilities/logging.h"
|
||||
#include "utilities/utilities.h"
|
||||
#include "servo_control/homing_controller.h"
|
||||
#include "servo_control/actuator_calibration.h"
|
||||
|
||||
//*** FUNCTION **************************************************************************
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
//--- RobotJoint ------------------------------------------------------------------------
|
||||
|
||||
RobotJoint::RobotJoint(MT6835Encoder* encoder,
|
||||
TB6612MotorDriver* motor_driver,
|
||||
int pole_pairs)
|
||||
{
|
||||
RobotJoint::encoder = encoder;
|
||||
RobotJoint::motor_driver = motor_driver;
|
||||
servo_controller = new ServoController(*motor_driver, *encoder, pole_pairs);
|
||||
position = 0.0f;
|
||||
velocity = 0.0f;
|
||||
}
|
||||
|
||||
RobotJoint::~RobotJoint() {
|
||||
delete servo_controller;
|
||||
delete motor_driver;
|
||||
delete encoder;
|
||||
servo_controller = nullptr;
|
||||
motor_driver = nullptr;
|
||||
encoder = nullptr;
|
||||
}
|
||||
|
||||
void RobotJoint::init(int joint_idx) {
|
||||
RobotJoint::joint_idx = joint_idx;
|
||||
|
||||
if(encoder->init(0x5, 0x4) == false) {
|
||||
LOG_ERROR("Failed to initialize encoder for joint %i", joint_idx);
|
||||
}
|
||||
|
||||
servo_controller->init(MOTOR_MAX_CURRENT_FACTOR);
|
||||
servo_controller->set_motor_enabled(false, false);
|
||||
}
|
||||
|
||||
bool RobotJoint::calibrate(bool print_measurements) {
|
||||
LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
|
||||
|
||||
HomingController homing_controller;
|
||||
bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY,
|
||||
360.0f*DEG_TO_RAD, HOMING_CURRENT,
|
||||
ENCODER_ANGLE_TO_ROTOR_ANGLE,
|
||||
0.0f);
|
||||
if(homing_ok == false) {
|
||||
LOG_ERROR("Joint-%i: Calibration failed due to unsuccessful homing sequence", joint_idx);
|
||||
return false;
|
||||
}
|
||||
|
||||
// measure lookup tables
|
||||
LookupTable encoder_raw_to_motor_pos_lut;
|
||||
LookupTable motor_pos_to_field_angle_lut;
|
||||
bool ok = measure_calibration_data(encoder_raw_to_motor_pos_lut,
|
||||
motor_pos_to_field_angle_lut,
|
||||
*servo_controller,
|
||||
CALIBRATION_RANGE*DEG_TO_RAD,
|
||||
CALIBRATION_FIELD_VELOCITY,
|
||||
256,
|
||||
print_measurements);
|
||||
if(!ok) {
|
||||
LOG_ERROR("Joint-%i: calibrating failed", joint_idx);
|
||||
return false;
|
||||
}
|
||||
|
||||
servo_controller->set_enc_to_pos_lut(encoder_raw_to_motor_pos_lut);
|
||||
servo_controller->set_pos_to_field_lut(motor_pos_to_field_angle_lut);
|
||||
is_calibrated = true;
|
||||
is_homed = true;
|
||||
|
||||
LOG_INFO("Joint-%i: calibrating joint successful.", joint_idx);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void RobotJoint::update(float dt, float one_over_dt) {
|
||||
servo_controller->update(position, dt, one_over_dt);
|
||||
}
|
||||
|
||||
void RobotJoint::update_target(float p, float v) {
|
||||
position = p;
|
||||
velocity = v;
|
||||
}
|
||||
|
||||
bool RobotJoint::load_calibration() {
|
||||
std::string fn1 = calib_data_filename("enc_to_pos_lut").c_str();
|
||||
std::string fn2 = calib_data_filename("pos_to_field_lut").c_str();
|
||||
if(!LittleFS.exists(fn1.c_str()) || !LittleFS.exists(fn2.c_str())) {
|
||||
LOG_WARNING("Joint-%i: Not all calibration files found. Run joint calibration with M56.", joint_idx);
|
||||
return false;
|
||||
}
|
||||
|
||||
LookupTable enc_to_pos_lut;
|
||||
LookupTable pos_to_field_lut;
|
||||
bool res = true;
|
||||
res &= load_lut_from_file(enc_to_pos_lut, fn1.c_str());
|
||||
res &= load_lut_from_file(pos_to_field_lut, fn2.c_str());
|
||||
if(res == false)
|
||||
return false;
|
||||
|
||||
servo_controller->set_enc_to_pos_lut(enc_to_pos_lut);
|
||||
servo_controller->set_pos_to_field_lut(pos_to_field_lut);
|
||||
|
||||
is_calibrated = true;
|
||||
LOG_INFO("Joint-%i: Encoder lookup tables loaded (size=%i,%i)",
|
||||
joint_idx, enc_to_pos_lut.size(), pos_to_field_lut.size());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RobotJoint::store_calibration() {
|
||||
bool res = true;
|
||||
|
||||
res &= save_lut_to_file(servo_controller->get_enc_to_pos_lut(),
|
||||
calib_data_filename("enc_to_pos_lut").c_str());
|
||||
res &= save_lut_to_file(servo_controller->get_pos_to_field_lut(),
|
||||
calib_data_filename("pos_to_field_lut").c_str());
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
std::string RobotJoint::calib_data_filename(std::string data_name) const {
|
||||
return std::string("joint")+std::to_string(joint_idx)+"_"+data_name+".dat";
|
||||
}
|
||||
60
firmware/MotionControllerRP/src/robot_joint/robot_joint.h
Normal file
60
firmware/MotionControllerRP/src/robot_joint/robot_joint.h
Normal file
|
|
@ -0,0 +1,60 @@
|
|||
// --------------------------------------------------------------------------------------
|
||||
// Project: MicroManipulatorStepper
|
||||
// License: MIT (see LICENSE file for full description)
|
||||
// All text in here must be included in any redistribution.
|
||||
// Author: M. S. (diffraction limited)
|
||||
// --------------------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "utilities/logging.h"
|
||||
#include "utilities/frequency_counter.h"
|
||||
#include "hardware/MT6701_encoder.h"
|
||||
#include "hardware/MT6835_encoder.h"
|
||||
#include "hardware/TB6612_motor_driver.h"
|
||||
#include "servo_control/servo_controller.h"
|
||||
#include "utilities/lookup_table.h"
|
||||
#include "utilities/math_constants.h"
|
||||
|
||||
#include "motion_control/path_planner.h"
|
||||
#include "motion_control/motion_controller.h"
|
||||
#include "command_parser/command_parser.h"
|
||||
#include "robot_joint_interface.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
//--- RobotJoint ------------------------------------------------------------------------
|
||||
|
||||
// TODO: create abstract interface for RobotJoint (to support remote Joints)
|
||||
// hide direct HW access
|
||||
|
||||
class RobotJoint {
|
||||
public:
|
||||
RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
|
||||
~RobotJoint();
|
||||
|
||||
void init(int joint_idx);
|
||||
bool calibrate(bool print_measurements);
|
||||
void update(float dt, float one_over_dt);
|
||||
void update_target(float p, float v);
|
||||
bool load_calibration();
|
||||
bool store_calibration();
|
||||
|
||||
|
||||
private:
|
||||
std::string calib_data_filename(std::string data_name) const;
|
||||
|
||||
public:
|
||||
int joint_idx = 0;
|
||||
bool is_homed = false;
|
||||
bool is_calibrated = false;
|
||||
|
||||
float position;
|
||||
float velocity;
|
||||
|
||||
MT6835Encoder* encoder;
|
||||
ServoController* servo_controller;
|
||||
|
||||
private:
|
||||
TB6612MotorDriver* motor_driver;
|
||||
};
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
// --------------------------------------------------------------------------------------
|
||||
// Project: MicroManipulatorStepper
|
||||
// License: MIT (see LICENSE file for full description)
|
||||
// All text in here must be included in any redistribution.
|
||||
// Author: M. S. (diffraction limited)
|
||||
// --------------------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
//--- RobotJoint ------------------------------------------------------------------------
|
||||
|
||||
// TODO: create abstract interface for RobotJoint (to support remote Joints)
|
||||
// hide direct HW access
|
||||
|
||||
class IRobotJoint {
|
||||
public:
|
||||
virtual ~IRobotJoint() {};
|
||||
|
||||
void init(int joint_idx);
|
||||
void update_target(float p, float v);
|
||||
bool load_calibration();
|
||||
bool store_calibration();
|
||||
|
||||
virtual bool calibrate(bool print_measurements) = 0;
|
||||
virtual void update(float dt, float one_over_dt) = 0;
|
||||
};
|
||||
|
|
@ -25,6 +25,8 @@ bool measure_calibration_data(
|
|||
size_t table_size,
|
||||
bool print_measurements)
|
||||
{
|
||||
const float max_rmse_rad = Constants::DEG2RAD * 0.5f;
|
||||
|
||||
LOG_INFO("Measuring motor to encoder angle lookup table...");
|
||||
int sample_count = table_size*4;
|
||||
|
||||
|
|
@ -37,7 +39,7 @@ bool measure_calibration_data(
|
|||
float start_field_angle = motor_driver.get_field_angle();
|
||||
float field_angle_step = calibration_range*pole_pair_count/(sample_count-1);
|
||||
|
||||
auto run_measurement = [&](int sample_count, float field_angle_step) {
|
||||
auto run_measurement = [&](int sample_count, float field_angle_step, int& weak_field_measurements) {
|
||||
// Measure in increasing direction
|
||||
for (size_t i = 0; i < sample_count; ++i) {
|
||||
if(i>0)
|
||||
|
|
@ -47,7 +49,10 @@ bool measure_calibration_data(
|
|||
float field_angle = motor_driver.get_field_angle();
|
||||
float motor_pos = (field_angle-start_field_angle)/pole_pair_count;
|
||||
// TODO: read motor_pos from precise reference encoder
|
||||
|
||||
|
||||
if(servo_controller.get_encoder().get_status() & MT6835_STATUS_WEAKFIELD)
|
||||
weak_field_measurements++;
|
||||
|
||||
if(print_measurements)
|
||||
LOG_RAW("%15.10f, %15.10f, %f", motor_pos, field_angle, encoder_angle_raw);
|
||||
|
||||
|
|
@ -57,10 +62,11 @@ bool measure_calibration_data(
|
|||
};
|
||||
|
||||
// Measure in increasing direction
|
||||
int weak_field_measurements = 0;
|
||||
LOG_DEBUG("Running foreward pass...");
|
||||
run_measurement(sample_count, field_angle_step);
|
||||
run_measurement(sample_count, field_angle_step, weak_field_measurements);
|
||||
LOG_DEBUG("Running backward pass...");
|
||||
run_measurement(sample_count, -field_angle_step);
|
||||
run_measurement(sample_count, -field_angle_step, weak_field_measurements);
|
||||
|
||||
// rotate back to start position
|
||||
motor_driver.rotate_field(start_field_angle-motor_driver.get_field_angle(),
|
||||
|
|
@ -68,20 +74,37 @@ bool measure_calibration_data(
|
|||
servo_controller.get_encoder().read_abs_angle_raw();
|
||||
});
|
||||
|
||||
// build lookup tables
|
||||
bool ok = encoder_raw_to_motor_pos_lut.init_interpolating(encoder_angle_and_motor_pos, table_size, true);
|
||||
encoder_raw_to_motor_pos_lut.optimize_lut(encoder_angle_and_motor_pos);
|
||||
if(ok == false) {
|
||||
LOG_ERROR("Creating lookup table encoder_raw_angle -> motor_pos failed.");
|
||||
if(weak_field_measurements > 0) {
|
||||
LOG_ERROR("Magnetic field too weak for %i of %i measurements", weak_field_measurements, sample_count);
|
||||
return false;
|
||||
}
|
||||
|
||||
ok = motor_pos_to_field_angle_lut.init_interpolating(motor_pos_and_field_angle, table_size/2, true);
|
||||
motor_pos_to_field_angle_lut.optimize_lut(motor_pos_and_field_angle);
|
||||
// build lookup table 'encoder_raw_angle -> motor_pos'
|
||||
float rmse = 0.0f;
|
||||
bool ok = encoder_raw_to_motor_pos_lut.init_interpolating(encoder_angle_and_motor_pos, table_size, true);
|
||||
if(ok == false) {
|
||||
LOG_ERROR("Creating lookup table motor_pos -> field_angle failed.");
|
||||
LOG_ERROR("Creating lookup table 'encoder_raw_angle -> motor_pos' failed.");
|
||||
return false;
|
||||
}
|
||||
encoder_raw_to_motor_pos_lut.optimize_lut(encoder_angle_and_motor_pos, rmse);
|
||||
if(rmse > max_rmse_rad) {
|
||||
LOG_WARNING("Fitting error of lookup table 'encoder_raw_angle -> motor_pos' unusually high (rms_error = %f deg)."
|
||||
"Calibration might be invalid.",
|
||||
Constants::RAD2DEG*rmse);
|
||||
}
|
||||
|
||||
// build lookup table 'motor_pos -> field_angle'
|
||||
ok = motor_pos_to_field_angle_lut.init_interpolating(motor_pos_and_field_angle, table_size/2, true);
|
||||
if(ok == false) {
|
||||
LOG_ERROR("Creating lookup table 'motor_pos -> field_angle' failed.");
|
||||
return false;
|
||||
}
|
||||
motor_pos_to_field_angle_lut.optimize_lut(motor_pos_and_field_angle, rmse);
|
||||
if(rmse > max_rmse_rad) {
|
||||
LOG_WARNING("Fitting error of lookup table 'motor_pos -> field_angle' is unusually high (rms_error = %f deg)."
|
||||
"Calibration might be invalid.",
|
||||
Constants::RAD2DEG*rmse);
|
||||
}
|
||||
|
||||
LOG_INFO("finished");
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ void HomingController::start(ServoController* servo_controller,
|
|||
float pole_pair_count = servo_controller->get_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)
|
||||
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
|
||||
|
|
@ -136,7 +136,7 @@ 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
|
||||
motor_driver.rotate_field(retract_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f),
|
||||
retract_field_velocity, [this](){
|
||||
|
|
|
|||
|
|
@ -12,6 +12,8 @@
|
|||
#include "utilities/logging.h"
|
||||
#include "utilities/math_constants.h"
|
||||
|
||||
#include "hw_config.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
ServoController::ServoController(
|
||||
|
|
@ -44,9 +46,14 @@ void ServoController::init(float max_motor_amplitude) {
|
|||
motor_driver.enable();
|
||||
motor_driver.set_field_angle(0.0f);
|
||||
|
||||
velocity_lowpass.set_time_constant(0.004f);
|
||||
pos_controller.set_parameter(75.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
|
||||
velocity_controller.set_parameter(0.2f, 150.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
|
||||
velocity_lowpass.set_time_constant(VEL_LOWPASS_TC);
|
||||
|
||||
pos_controller.set_parameter(POS_KP, POS_KI, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
|
||||
velocity_controller.set_parameter(VEL_KP, VEL_KI, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
|
||||
|
||||
// large 0.9° steppers
|
||||
// pos_controller.set_parameter(75.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
|
||||
// velocity_controller.set_parameter(0.2f, 150.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
|
||||
|
||||
// pos_controller.set_parameter(75.0f, 2000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
|
||||
// velocity_controller.set_parameter(0.2f, 0.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
|
||||
|
|
|
|||
|
|
@ -147,7 +147,7 @@ bool LookupTable::init_interpolating(std::vector<std::pair<float, float>> in_out
|
|||
return true;
|
||||
}
|
||||
|
||||
bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs) {
|
||||
bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs, float& rms_error) {
|
||||
if (lookup_table.empty() || in_out_pairs.empty())
|
||||
return false;
|
||||
|
||||
|
|
@ -198,7 +198,9 @@ bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs
|
|||
//if(iter%100 == 0)
|
||||
// LOG_DEBUG("iteration %04i: rms=%f", iter, sqrtf(total_loss));
|
||||
}
|
||||
LOG_DEBUG("Optimizing lookup table finished: rms=%f", total_loss);
|
||||
|
||||
rms_error = sqrt(total_loss);
|
||||
LOG_DEBUG("Optimizing lookup table finished: rms_error=%f", rms_error);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ class LookupTable {
|
|||
int table_size, bool sort_input);
|
||||
|
||||
// optimizes the lookup table using gradient descen to minimize error to provided data
|
||||
bool optimize_lut(std::vector<std::pair<float, float>> in_out_pairs);
|
||||
bool optimize_lut(std::vector<std::pair<float, float>> in_out_pairs, float& rms_error);
|
||||
|
||||
// clear the lookup table, use init to use it again
|
||||
void clear();
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
static const char* FIRMWARE_VERSION = "v1.0.3";
|
||||
static const char* FIRMWARE_VERSION = "v1.0.4";
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue