added files
This commit is contained in:
0x23
parent
36090af63b
commit
4cd8e6e20e
86 changed files with 74246 additions and 0 deletions
|
|
@ -0,0 +1,138 @@
|
|||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "command_parser.h"
|
||||
#include <cstring>
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
//--- GCodeCommand ----------------------------------------------------------------------
|
||||
|
||||
GCodeCommand::GCodeCommand() {
|
||||
reset();
|
||||
}
|
||||
|
||||
void GCodeCommand::set_command(const char* cmd) {
|
||||
if (cmd) {
|
||||
command = cmd;
|
||||
} else {
|
||||
command = "";
|
||||
}
|
||||
}
|
||||
|
||||
const std::string& GCodeCommand::get_command() const {
|
||||
return command;
|
||||
}
|
||||
|
||||
void GCodeCommand::reset() {
|
||||
// Initialize all word values to NaN to represent "not set"
|
||||
for (float& value : word_values) {
|
||||
value = std::numeric_limits<float>::quiet_NaN();
|
||||
}
|
||||
command[0] = 0;
|
||||
}
|
||||
|
||||
void GCodeCommand::set_value(char word, float value) {
|
||||
if (word >= 'A' && word <= 'Z') {
|
||||
word_values[word-'A'] = value;
|
||||
}
|
||||
}
|
||||
|
||||
float GCodeCommand::get_value(char word) const {
|
||||
return get_value(word, std::numeric_limits<float>::quiet_NaN());
|
||||
}
|
||||
|
||||
float GCodeCommand::get_value(char word, float default_value) const {
|
||||
if (word >= 'A' && word <= 'Z') {
|
||||
float value = word_values[word-'A'];
|
||||
return std::isnan(value) ? default_value : value;
|
||||
}
|
||||
return default_value;
|
||||
}
|
||||
|
||||
bool GCodeCommand::has_word(char word) const {
|
||||
if (word >= 'A' && word <= 'Z') {
|
||||
return !std::isnan(word_values[word-'A']);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
//--- CommandParser ---------------------------------------------------------------------
|
||||
|
||||
CommandParser::CommandParser() : buffer_index(0), command_processor(nullptr) {
|
||||
|
||||
}
|
||||
|
||||
void CommandParser::set_command_processor(ICommandProcessor* cp) {
|
||||
command_processor = cp;
|
||||
}
|
||||
|
||||
void CommandParser::update() {
|
||||
if(command_processor == nullptr)
|
||||
return;
|
||||
|
||||
if(command_ready == false)
|
||||
return;
|
||||
|
||||
// Call user callback with parsed command
|
||||
if(command_processor->can_process_command(command)) {
|
||||
std::string reply;
|
||||
command_processor->process_command(command, reply);
|
||||
command_processor->send_reply(reply.c_str());
|
||||
|
||||
// mark command as processed
|
||||
command_ready = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Feed input chars one by one
|
||||
void CommandParser::add_input_character(char c) {
|
||||
if (c == '\n' || c == '\r') {
|
||||
if (buffer_index > 0) {
|
||||
buffer[buffer_index] = '\0';
|
||||
parse_line(buffer);
|
||||
buffer_index = 0;
|
||||
}
|
||||
} else if (buffer_index < sizeof(buffer) - 1) {
|
||||
buffer[buffer_index++] = 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';
|
||||
|
||||
// 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') {
|
||||
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') {
|
||||
command.set_value(token[0], strtof(token + 1, nullptr));
|
||||
} else {
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
#pragma once
|
||||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include "utilities/math3d.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
//--- GCodeCommand ----------------------------------------------------------------------
|
||||
|
||||
class GCodeCommand {
|
||||
public:
|
||||
GCodeCommand();
|
||||
|
||||
void reset();
|
||||
void set_command(const char* cmd);
|
||||
const std::string& get_command() const;
|
||||
void set_value(char word, float value);
|
||||
float get_value(char word) const;
|
||||
float get_value(char word, float default_value) const;
|
||||
bool has_word(char word) const;
|
||||
|
||||
private:
|
||||
std::string command;
|
||||
float word_values[26];
|
||||
};
|
||||
|
||||
//--- ICommandProcessor -----------------------------------------------------------------
|
||||
|
||||
class ICommandProcessor {
|
||||
public:
|
||||
virtual ~ICommandProcessor() {};
|
||||
|
||||
virtual void send_reply(const char* str) = 0;
|
||||
virtual bool can_process_command(const GCodeCommand& cmd) = 0;
|
||||
virtual void process_command(const GCodeCommand& cmd, std::string& reply) = 0;
|
||||
};
|
||||
|
||||
//--- CommandParser ---------------------------------------------------------------------
|
||||
|
||||
class CommandParser {
|
||||
public:
|
||||
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:
|
||||
bool parse_line(const char* line);
|
||||
bool handle_gcode_command(const GCodeCommand& cmd);
|
||||
|
||||
private:
|
||||
char buffer[255];
|
||||
int buffer_index;
|
||||
ICommandProcessor* command_processor;
|
||||
|
||||
GCodeCommand command;
|
||||
bool command_ready;
|
||||
};
|
||||
|
||||
79
firmware/MotionControllerRP/src/hardware/MT6701_encoder.cpp
Normal file
79
firmware/MotionControllerRP/src/hardware/MT6701_encoder.cpp
Normal file
|
|
@ -0,0 +1,79 @@
|
|||
#include "MT6701_encoder.h"
|
||||
|
||||
MT6701Encoder::MT6701Encoder(TwoWire& wire, uint8_t i2c_addr)
|
||||
: wire(wire), address(i2c_addr) {
|
||||
}
|
||||
|
||||
void MT6701Encoder::init() {
|
||||
wire.begin();
|
||||
last_raw_angle = 0;
|
||||
abs_raw_angle = 0;
|
||||
}
|
||||
|
||||
int32_t MT6701Encoder::read_abs_angle_raw() {
|
||||
wire.beginTransmission(address);
|
||||
wire.write(0x03); // ANGLE_H register
|
||||
wire.endTransmission(false);
|
||||
|
||||
wire.requestFrom(address, (uint8_t)2);
|
||||
if (wire.available() < 2) return -1;
|
||||
|
||||
uint8_t angle_h = wire.read();
|
||||
uint8_t angle_l = wire.read();
|
||||
|
||||
int32_t raw_angle = (angle_h << 6) | (angle_l >> 2);
|
||||
return update_abs_raw_angle(raw_angle);
|
||||
}
|
||||
|
||||
float MT6701Encoder::read_abs_angle() {
|
||||
int32_t raw = read_abs_angle_raw();
|
||||
return float(raw) * RAW_TO_RAD;
|
||||
}
|
||||
|
||||
void MT6701Encoder::set_hysteresis(uint8_t hyst) {
|
||||
if (hyst > 7) return;
|
||||
|
||||
uint8_t hyst2 = (hyst >> 2) & 0x01;
|
||||
uint8_t hyst10 = hyst & 0x03;
|
||||
|
||||
// --- Register 0x32 ---
|
||||
wire.beginTransmission(address);
|
||||
wire.write(0x32);
|
||||
wire.endTransmission(false);
|
||||
wire.requestFrom(address, (uint8_t)1);
|
||||
uint8_t reg32 = wire.read();
|
||||
reg32 = (reg32 & 0x7F) | (hyst2 << 7);
|
||||
|
||||
wire.beginTransmission(address);
|
||||
wire.write(0x32);
|
||||
wire.write(reg32);
|
||||
wire.endTransmission();
|
||||
|
||||
// --- Register 0x34 ---
|
||||
wire.beginTransmission(address);
|
||||
wire.write(0x34);
|
||||
wire.endTransmission(false);
|
||||
wire.requestFrom(address, (uint8_t)1);
|
||||
uint8_t reg34 = wire.read();
|
||||
reg34 = (reg34 & 0x3F) | (hyst10 << 6);
|
||||
|
||||
wire.beginTransmission(address);
|
||||
wire.write(0x34);
|
||||
wire.write(reg34);
|
||||
wire.endTransmission();
|
||||
}
|
||||
|
||||
MT6701Encoder::AbsRawAngleType MT6701Encoder::update_abs_raw_angle(int32_t raw_angle) {
|
||||
if (raw_angle >= 0) {
|
||||
int32_t half_max = CPR >> 1;
|
||||
int d = raw_angle - last_raw_angle;
|
||||
|
||||
if (d > half_max) d -= CPR;
|
||||
else if (d < -half_max) d += CPR;
|
||||
|
||||
abs_raw_angle += d;
|
||||
last_raw_angle = raw_angle;
|
||||
}
|
||||
|
||||
return abs_raw_angle;
|
||||
}
|
||||
27
firmware/MotionControllerRP/src/hardware/MT6701_encoder.h
Normal file
27
firmware/MotionControllerRP/src/hardware/MT6701_encoder.h
Normal file
|
|
@ -0,0 +1,27 @@
|
|||
#pragma once
|
||||
|
||||
#include <Wire.h>
|
||||
|
||||
class MT6701Encoder {
|
||||
public:
|
||||
using AbsRawAngleType = int32_t;
|
||||
|
||||
MT6701Encoder(TwoWire& wire, uint8_t i2c_addr=0x06);
|
||||
void init();
|
||||
|
||||
int32_t read_abs_angle_raw();
|
||||
float read_abs_angle();
|
||||
|
||||
void set_hysteresis(uint8_t hyst); // 0–7
|
||||
AbsRawAngleType update_abs_raw_angle(int32_t raw_angle);
|
||||
|
||||
private:
|
||||
TwoWire& wire;
|
||||
uint8_t address;
|
||||
|
||||
AbsRawAngleType abs_raw_angle = 0;
|
||||
int32_t last_raw_angle = -1;
|
||||
|
||||
static constexpr int CPR = 16384; // 14-bit resolution
|
||||
static constexpr float RAW_TO_RAD = 2*PI / CPR;
|
||||
};
|
||||
312
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
Normal file
312
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
Normal file
|
|
@ -0,0 +1,312 @@
|
|||
#include "MT6835_encoder.h"
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "pico/stdlib.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
|
||||
gpio_set_function(pin_sck, GPIO_FUNC_SPI);
|
||||
gpio_set_function(pin_mosi, GPIO_FUNC_SPI);
|
||||
gpio_set_function(pin_miso, GPIO_FUNC_SPI);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
||||
MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
|
||||
// nop
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
set_bandwidth(bandwidth);
|
||||
set_hysteresis(hysteresis);
|
||||
|
||||
last_raw_angle = 0;
|
||||
abs_raw_angle = 0;
|
||||
}
|
||||
|
||||
void MT6835Encoder::reset_abs_angle(int32_t abs_raw_angle) {
|
||||
MT6835Encoder::abs_raw_angle = abs_raw_angle;
|
||||
}
|
||||
|
||||
float MT6835Encoder::read_abs_angle() {
|
||||
int32_t raw_angle = read_abs_angle_raw();
|
||||
return raw_angle * RAW_TO_ANGLE;
|
||||
}
|
||||
|
||||
int32_t MT6835Encoder::read_abs_angle_raw() {
|
||||
uint8_t data[6] = {0};
|
||||
data[0] = MT6835_OP_ANGLE << 4;
|
||||
data[1] = MT6835_REG_ANGLE1;
|
||||
// rest zero
|
||||
|
||||
spi_begin_transaction();
|
||||
spi_transfer(data, 6);
|
||||
spi_end_transaction();
|
||||
|
||||
last_status = data[4] & 0x07;
|
||||
last_crc = data[5];
|
||||
int32_t raw_angle = ((int32_t)data[2] << 13) | ((int32_t)data[3] << 5) | (data[4] >> 3);
|
||||
|
||||
if (check_crc) {
|
||||
if (last_crc != calc_crc(raw_angle, last_status)) {
|
||||
last_status |= MT6835_CRC_ERROR;
|
||||
return -1.0f; // CRC error indicator
|
||||
}
|
||||
}
|
||||
|
||||
return update_abs_raw_angle(raw_angle);
|
||||
}
|
||||
|
||||
int32_t MT6835Encoder::get_rawcounts_per_rev() {
|
||||
return MT6835_CPR;
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_status() {
|
||||
return last_status;
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_calibration_status() {
|
||||
uint8_t data[3] = {0};
|
||||
data[0] = (MT6835_OP_READ << 4) | (MT6835_REG_CAL_STATUS >> 8);
|
||||
data[1] = MT6835_REG_CAL_STATUS & 0xFF;
|
||||
|
||||
spi_begin_transaction();
|
||||
spi_transfer(data, 3);
|
||||
spi_end_transaction();
|
||||
|
||||
return data[2] >> 6;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::set_zero_from_current_position() {
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_ZERO;
|
||||
cmd.addr = 0x000;
|
||||
transfer_24(&cmd);
|
||||
abs_raw_angle = 0;
|
||||
last_raw_angle = 0;
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::write_eeprom() {
|
||||
sleep_ms(1); // wait at least 1 ms
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_PROG;
|
||||
cmd.addr = 0x000;
|
||||
transfer_24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_bandwidth() {
|
||||
MT6835Options5 opts{ .reg = read_register(MT6835_REG_OPTS5) };
|
||||
return opts.bw;
|
||||
}
|
||||
void MT6835Encoder::set_bandwidth(uint8_t bw) {
|
||||
MT6835Options5 opts{ .reg = read_register(MT6835_REG_OPTS5) };
|
||||
opts.bw = bw;
|
||||
write_register(MT6835_REG_OPTS5, opts.reg);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_hysteresis() {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
return opts.hyst;
|
||||
}
|
||||
void MT6835Encoder::set_hysteresis(uint8_t hyst) {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
opts.hyst = hyst;
|
||||
set_options3(opts);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_rotation_direction() {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
return opts.rot_dir;
|
||||
}
|
||||
void MT6835Encoder::set_rotation_direction(uint8_t dir) {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
opts.rot_dir = dir;
|
||||
set_options3(opts);
|
||||
}
|
||||
|
||||
uint16_t MT6835Encoder::get_abz_resolution() {
|
||||
uint8_t hi = read_register(MT6835_REG_ABZ_RES1);
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
return (hi << 6) | lo.abz_res_low;
|
||||
}
|
||||
void MT6835Encoder::set_abz_resolution(uint16_t res) {
|
||||
uint8_t hi = (res >> 6);
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
lo.abz_res_low = (res & 0x3F);
|
||||
write_register(MT6835_REG_ABZ_RES1, hi);
|
||||
write_register(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
}
|
||||
|
||||
bool MT6835Encoder::is_abz_enabled() {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
return lo.abz_off == 0;
|
||||
}
|
||||
void MT6835Encoder::set_abz_enabled(bool enabled) {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
lo.abz_off = enabled ? 0 : 1;
|
||||
write_register(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
}
|
||||
|
||||
bool MT6835Encoder::is_ab_swapped() {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
return lo.ab_swap == 1;
|
||||
}
|
||||
void MT6835Encoder::set_ab_swapped(bool swapped) {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
lo.ab_swap = swapped ? 1 : 0;
|
||||
write_register(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
}
|
||||
|
||||
uint16_t MT6835Encoder::get_zero_position() {
|
||||
uint8_t hi = read_register(MT6835_REG_ZERO1);
|
||||
MT6835Options0 lo{ .reg = read_register(MT6835_REG_ZERO2) };
|
||||
return (hi << 4) | lo.zero_pos_low;
|
||||
}
|
||||
void MT6835Encoder::set_zero_position(uint16_t pos) {
|
||||
uint8_t hi = (pos >> 4);
|
||||
MT6835Options0 lo{ .reg = read_register(MT6835_REG_ZERO2) };
|
||||
lo.zero_pos_low = pos & 0x0F;
|
||||
write_register(MT6835_REG_ZERO1, hi);
|
||||
write_register(MT6835_REG_ZERO2, lo.reg);
|
||||
}
|
||||
|
||||
MT6835Options1 MT6835Encoder::get_options1() {
|
||||
MT6835Options1 result{ .reg = read_register(MT6835_REG_OPTS1) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options1(MT6835Options1 opts) {
|
||||
write_register(MT6835_REG_OPTS1, opts.reg);
|
||||
}
|
||||
|
||||
MT6835Options2 MT6835Encoder::get_options2() {
|
||||
MT6835Options2 result{ .reg = read_register(MT6835_REG_OPTS2) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options2(MT6835Options2 opts) {
|
||||
MT6835Options2 val = get_options2();
|
||||
val.nlc_en = opts.nlc_en;
|
||||
val.pwm_fq = opts.pwm_fq;
|
||||
val.pwm_pol = opts.pwm_pol;
|
||||
val.pwm_sel = opts.pwm_sel;
|
||||
write_register(MT6835_REG_OPTS2, val.reg);
|
||||
}
|
||||
|
||||
MT6835Options3 MT6835Encoder::get_options3() {
|
||||
MT6835Options3 result{ .reg = read_register(MT6835_REG_OPTS3) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options3(MT6835Options3 opts) {
|
||||
MT6835Options3 val = get_options3();
|
||||
val.rot_dir = opts.rot_dir;
|
||||
val.hyst = opts.hyst;
|
||||
write_register(MT6835_REG_OPTS3, val.reg);
|
||||
}
|
||||
|
||||
MT6835Options4 MT6835Encoder::get_options4() {
|
||||
MT6835Options4 result{ .reg = read_register(MT6835_REG_OPTS4) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options4(MT6835Options4 opts) {
|
||||
MT6835Options4 val = get_options4();
|
||||
val.gpio_ds = opts.gpio_ds;
|
||||
val.autocal_freq = opts.autocal_freq;
|
||||
write_register(MT6835_REG_OPTS4, val.reg);
|
||||
}
|
||||
|
||||
static inline uint32_t swap_bytes(uint32_t val) {
|
||||
return __builtin_bswap32(val);
|
||||
}
|
||||
|
||||
void MT6835Encoder::transfer_24(MT6835Command* cmd) {
|
||||
uint32_t buff = swap_bytes(cmd->val);
|
||||
spi_begin_transaction();
|
||||
spi_transfer((uint8_t*)&buff, 3);
|
||||
spi_end_transaction();
|
||||
cmd->val = swap_bytes(buff);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::read_register(uint16_t reg) {
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_READ;
|
||||
cmd.addr = reg;
|
||||
transfer_24(&cmd);
|
||||
return cmd.data;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::write_register(uint16_t reg, uint8_t value) {
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_WRITE;
|
||||
cmd.addr = reg;
|
||||
cmd.data = value;
|
||||
transfer_24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
}
|
||||
|
||||
MT6835Encoder::AbsRawAngleType MT6835Encoder::update_abs_raw_angle(int32_t raw_angle) {
|
||||
if(raw_angle >= 0) {
|
||||
int32_t half_max = MT6835_CPR>>1;
|
||||
|
||||
int32_t d = raw_angle - last_raw_angle;
|
||||
if (d > half_max) d -= MT6835_CPR;
|
||||
else if (d < -half_max) d += MT6835_CPR;
|
||||
|
||||
abs_raw_angle += d;
|
||||
last_raw_angle = raw_angle;
|
||||
}
|
||||
|
||||
return abs_raw_angle;
|
||||
}
|
||||
|
||||
// Helper SPI transaction helpers for CS handling and SPI transfer
|
||||
void MT6835Encoder::spi_begin_transaction() {
|
||||
// No real beginTransaction in Pico SDK; just pull CS low if used
|
||||
if (cs_pin >= 0)
|
||||
gpio_put(cs_pin, 0);
|
||||
}
|
||||
|
||||
void MT6835Encoder::spi_transfer(uint8_t* data, size_t length) {
|
||||
// Full-duplex transfer, sending and receiving on SPI
|
||||
spi_write_read_blocking(spi, data, data, length);
|
||||
}
|
||||
|
||||
void MT6835Encoder::spi_end_transaction() {
|
||||
if (cs_pin >= 0)
|
||||
gpio_put(cs_pin, 1);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::calc_crc(uint32_t angle, uint8_t status) {
|
||||
uint8_t crc = 0x00;
|
||||
uint8_t input;
|
||||
|
||||
input = angle >> 13;
|
||||
crc ^= input;
|
||||
for (int k = 8; k > 0; k--)
|
||||
crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
|
||||
|
||||
input = (angle >> 5) & 0xFF;
|
||||
crc ^= input;
|
||||
for (int k = 8; k > 0; k--)
|
||||
crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
|
||||
|
||||
input = ((angle << 3) & 0xFF) | (status & 0x07);
|
||||
crc ^= input;
|
||||
for (int k = 8; k > 0; k--)
|
||||
crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
|
||||
|
||||
return crc;
|
||||
}
|
||||
212
firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
Normal file
212
firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
Normal file
|
|
@ -0,0 +1,212 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/gpio.h"
|
||||
|
||||
#define MT6835_CPR (1<<21)
|
||||
|
||||
#define MT6835_OP_READ 0b0011
|
||||
#define MT6835_OP_WRITE 0b0110
|
||||
#define MT6835_OP_PROG 0b1100
|
||||
#define MT6835_OP_ZERO 0b0101
|
||||
#define MT6835_OP_ANGLE 0b1010
|
||||
|
||||
#define MT6835_CMD_MASK 0b111100000000000000000000
|
||||
#define MT6835_ADDR_MASK 0b000011111111111100000000
|
||||
#define MT6835_DATA_MASK 0b000000000000000011111111
|
||||
|
||||
#define MT6835_STATUS_OVERSPEED 0x01
|
||||
#define MT6835_STATUS_WEAKFIELD 0x02
|
||||
#define MT6835_STATUS_UNDERVOLT 0x04
|
||||
#define MT6835_CRC_ERROR 0x08
|
||||
|
||||
#define MT6835_WRITE_ACK 0x55
|
||||
|
||||
#define MT6835_REG_USERID 0x001
|
||||
|
||||
#define MT6835_REG_ANGLE1 0x003
|
||||
#define MT6835_REG_ANGLE2 0x004
|
||||
#define MT6835_REG_ANGLE3 0x005
|
||||
#define MT6835_REG_ANGLE4 0x006
|
||||
|
||||
#define MT6835_REG_ABZ_RES1 0x007
|
||||
#define MT6835_REG_ABZ_RES2 0x008
|
||||
|
||||
#define MT6835_REG_ZERO1 0x009
|
||||
#define MT6835_REG_ZERO2 0x00A
|
||||
|
||||
#define MT6835_REG_OPTS0 0x00A
|
||||
#define MT6835_REG_OPTS1 0x00B
|
||||
#define MT6835_REG_OPTS2 0x00C
|
||||
#define MT6835_REG_OPTS3 0x00D
|
||||
#define MT6835_REG_OPTS4 0x00E
|
||||
#define MT6835_REG_OPTS5 0x011
|
||||
|
||||
// NLC table, 192 bytes
|
||||
#define MT6835_REG_NLC_BASE 0x013
|
||||
#define MT6835_REG_CAL_STATUS 0x113
|
||||
|
||||
//*** DATATYPES **********************************************************************************/
|
||||
|
||||
union MT6835ABZRes {
|
||||
struct {
|
||||
uint8_t ab_swap:1;
|
||||
uint8_t abz_off:1;
|
||||
uint8_t abz_res_low:6;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Options0 {
|
||||
struct {
|
||||
uint8_t z_pul_wid:3;
|
||||
uint8_t z_edge:1;
|
||||
uint8_t zero_pos_low:4;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Options1 {
|
||||
struct {
|
||||
uint8_t uvw_res:4;
|
||||
uint8_t uvw_off:1;
|
||||
uint8_t uvw_mux:1;
|
||||
uint8_t z_phase:2;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Options2 {
|
||||
struct {
|
||||
uint8_t pwm_sel:3;
|
||||
uint8_t pwm_pol:1;
|
||||
uint8_t pwm_fq:1;
|
||||
uint8_t nlc_en:1;
|
||||
uint8_t reserved:2;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Options3 {
|
||||
struct {
|
||||
uint8_t hyst:3;
|
||||
uint8_t rot_dir:1;
|
||||
uint8_t reserved:4;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Options4 {
|
||||
struct {
|
||||
uint8_t reserved:4;
|
||||
uint8_t autocal_freq:3;
|
||||
uint8_t gpio_ds:1;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Options5 {
|
||||
struct {
|
||||
uint8_t bw:3;
|
||||
uint8_t reserved:5;
|
||||
};
|
||||
uint8_t reg;
|
||||
};
|
||||
|
||||
union MT6835Command {
|
||||
struct {
|
||||
uint32_t unused:8;
|
||||
uint32_t data:8;
|
||||
uint32_t addr:12;
|
||||
uint32_t cmd:4;
|
||||
};
|
||||
uint32_t val;
|
||||
};
|
||||
|
||||
//*** CLASS ************************************************************************************/
|
||||
|
||||
class MT6835Encoder {
|
||||
public:
|
||||
typedef int32_t AbsRawAngleType;
|
||||
|
||||
public:
|
||||
static constexpr float RAW_TO_ANGLE = (2.0f*3.14159265358979323846f)/float(MT6835_CPR);
|
||||
|
||||
// use this to setup a HW spi. It can then be used by multiple instances of MT6835
|
||||
static void setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz);
|
||||
|
||||
// Constructor: pass SPI instance (spi0 or spi1), CS pin
|
||||
MT6835Encoder(spi_inst_t *spi, uint cs_pin);
|
||||
virtual ~MT6835Encoder();
|
||||
|
||||
void init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
|
||||
void reset_abs_angle(int32_t abs_raw_angle=0); // resets the total revolutions of abs angle
|
||||
|
||||
float read_abs_angle(); // returns the absolute angle in radians
|
||||
AbsRawAngleType read_abs_angle_raw(); // returns the absolute angle in raw counts
|
||||
|
||||
int32_t get_rawcounts_per_rev(); // returns the number of raw counts per revolution
|
||||
|
||||
uint8_t get_bandwidth();
|
||||
void set_bandwidth(uint8_t bw);
|
||||
|
||||
uint8_t get_hysteresis();
|
||||
void set_hysteresis(uint8_t hyst);
|
||||
|
||||
uint8_t get_rotation_direction();
|
||||
void set_rotation_direction(uint8_t dir);
|
||||
|
||||
uint16_t get_abz_resolution();
|
||||
void set_abz_resolution(uint16_t res);
|
||||
|
||||
bool is_abz_enabled();
|
||||
void set_abz_enabled(bool enabled);
|
||||
|
||||
bool is_ab_swapped();
|
||||
void set_ab_swapped(bool swapped);
|
||||
|
||||
uint16_t get_zero_position();
|
||||
void set_zero_position(uint16_t pos);
|
||||
|
||||
MT6835Options1 get_options1();
|
||||
void set_options1(MT6835Options1 opts);
|
||||
|
||||
MT6835Options2 get_options2();
|
||||
void set_options2(MT6835Options2 opts);
|
||||
|
||||
MT6835Options3 get_options3();
|
||||
void set_options3(MT6835Options3 opts);
|
||||
|
||||
MT6835Options4 get_options4();
|
||||
void set_options4(MT6835Options4 opts);
|
||||
|
||||
uint8_t get_status();
|
||||
|
||||
uint8_t get_calibration_status();
|
||||
|
||||
bool set_zero_from_current_position();
|
||||
bool write_eeprom(); // takes ~6s to complete after calling
|
||||
|
||||
bool check_crc = false;
|
||||
|
||||
private:
|
||||
spi_inst_t *spi;
|
||||
uint cs_pin;
|
||||
uint8_t last_status = 0;
|
||||
uint8_t last_crc = 0;
|
||||
|
||||
int32_t last_raw_angle = 0;
|
||||
AbsRawAngleType abs_raw_angle = 0;
|
||||
|
||||
AbsRawAngleType update_abs_raw_angle(int32_t raw_angle);
|
||||
|
||||
void spi_begin_transaction();
|
||||
void spi_transfer(uint8_t* data, size_t length);
|
||||
void spi_end_transaction();
|
||||
|
||||
void transfer_24(MT6835Command *out_value);
|
||||
uint8_t read_register(uint16_t reg);
|
||||
bool write_register(uint16_t reg, uint8_t value);
|
||||
uint8_t calc_crc(uint32_t angle, uint8_t status);
|
||||
};
|
||||
114
firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
Normal file
114
firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
Normal file
|
|
@ -0,0 +1,114 @@
|
|||
#include "TB6612_motor_driver.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include "hardware/pwm.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "hardware/clocks.h"
|
||||
|
||||
// Helper rounding function
|
||||
int32_t round_int32(float val) {
|
||||
return (val >= 0.0f) ? (int32_t)(val + 0.5f) : (int32_t)(val - 0.5f);
|
||||
}
|
||||
|
||||
TB6612MotorDriver::TB6612MotorDriver(
|
||||
uint8_t pin_en_a, uint8_t pin_pos_a, uint8_t pin_neg_a, uint8_t pin_pwm_a,
|
||||
uint8_t pin_en_b, uint8_t pin_pos_b, uint8_t pin_neg_b, uint8_t pin_pwm_b,
|
||||
uint8_t ch_pos_a, uint8_t ch_neg_a,
|
||||
uint8_t ch_pos_b, uint8_t ch_neg_b,
|
||||
uint16_t pwm_freq,
|
||||
uint8_t pwm_resolution
|
||||
)
|
||||
: pin_en_a(pin_en_a), pin_pos_a(pin_pos_a), pin_neg_a(pin_neg_a), pin_pwm_a(pin_pwm_a),
|
||||
pin_en_b(pin_en_b), pin_pos_b(pin_pos_b), pin_neg_b(pin_neg_b), pin_pwm_b(pin_pwm_b),
|
||||
ch_pos_a(ch_pos_a), ch_neg_a(ch_neg_a),
|
||||
ch_pos_b(ch_pos_b), ch_neg_b(ch_neg_b),
|
||||
pwm_freq(pwm_freq), pwm_resolution(pwm_resolution)
|
||||
{
|
||||
max_pwm = (1 << pwm_resolution) - 1;
|
||||
amplitude = 0.1f * max_pwm; // Default to 10% amplitude
|
||||
}
|
||||
|
||||
void init_output_pin(uint8_t pin, bool value) {
|
||||
gpio_init(pin);
|
||||
gpio_set_dir(pin, GPIO_OUT);
|
||||
gpio_put(pin, value);
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::begin() {
|
||||
init_output_pin(pin_en_a, false);
|
||||
init_output_pin(pin_en_b, false);
|
||||
|
||||
// enable pwm pins, see TB6612 documentation for how the PWM pins work,
|
||||
// for slow decay mode they are constantly enabled
|
||||
init_output_pin(pin_pwm_a, true);
|
||||
init_output_pin(pin_pwm_b, true);
|
||||
|
||||
// not needed since pwm pins are configured below
|
||||
// pinMode(pin_pos_a, OUTPUT);
|
||||
// pinMode(pin_neg_a, OUTPUT);
|
||||
// pinMode(pin_pos_b, OUTPUT);
|
||||
// pinMode(pin_neg_b, OUTPUT);
|
||||
|
||||
// Read system clock dynamically
|
||||
uint32_t sys_clk = clock_get_hz(clk_sys);
|
||||
float clkdiv = (float)sys_clk / (pwm_freq * max_pwm);
|
||||
|
||||
// Setup helper
|
||||
auto setup_pwm_pin = [&](uint8_t pin) {
|
||||
gpio_set_function(pin, GPIO_FUNC_PWM);
|
||||
uint slice = pwm_gpio_to_slice_num(pin);
|
||||
uint chan = pwm_gpio_to_channel(pin); // 0 for A, 1 for B
|
||||
pwm_config config = pwm_get_default_config();
|
||||
pwm_config_set_clkdiv(&config, clkdiv);
|
||||
pwm_config_set_wrap(&config, max_pwm);
|
||||
pwm_init(slice, &config, true);
|
||||
};
|
||||
|
||||
setup_pwm_pin(pin_pos_a);
|
||||
setup_pwm_pin(pin_neg_a);
|
||||
setup_pwm_pin(pin_pos_b);
|
||||
setup_pwm_pin(pin_neg_b);
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::enable() {
|
||||
gpio_put(pin_en_a, 1);
|
||||
gpio_put(pin_en_b, 1);
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::disable() {
|
||||
gpio_put(pin_en_a, 0);
|
||||
gpio_put(pin_en_b, 0);
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::set_amplitude(float amplitude, bool immediate_update) {
|
||||
amplitude = std::clamp(amplitude, 0.0f, 1.0f);
|
||||
TB6612MotorDriver::amplitude = amplitude * max_pwm;
|
||||
if(immediate_update)
|
||||
set_field_angle(field_angle);
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::set_field_angle(float angle_rad) {
|
||||
field_angle = angle_rad;
|
||||
float sin_a = sin(angle_rad);
|
||||
float cos_a = cos(angle_rad);
|
||||
|
||||
set_pwm(pin_pos_a, pin_neg_a, round_int32(sin_a * amplitude));
|
||||
set_pwm(pin_pos_b, pin_neg_b, round_int32(cos_a * amplitude));
|
||||
}
|
||||
|
||||
float TB6612MotorDriver::get_field_angle() {
|
||||
return field_angle;
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::set_pwm(uint8_t pin_pos, uint8_t pin_neg, int32_t value) {
|
||||
value = std::min(std::max(value, -(int32_t)max_pwm), (int32_t)max_pwm);
|
||||
|
||||
if (value >= 0) {
|
||||
pwm_set_gpio_level(pin_pos, max_pwm - value);
|
||||
pwm_set_gpio_level(pin_neg, max_pwm);
|
||||
} else {
|
||||
pwm_set_gpio_level(pin_pos, max_pwm);
|
||||
pwm_set_gpio_level(pin_neg, max_pwm + value);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
// #include <Arduino.h>
|
||||
|
||||
class TB6612MotorDriver {
|
||||
public:
|
||||
TB6612MotorDriver(
|
||||
uint8_t pin_en_a, uint8_t pin_pos_a, uint8_t pin_neg_a, uint8_t pin_pwm_a,
|
||||
uint8_t pin_en_b, uint8_t pin_pos_b, uint8_t pin_neg_b, uint8_t pin_pwm_b,
|
||||
uint8_t ch_pos_a=0, uint8_t ch_neg_a=1,
|
||||
uint8_t ch_pos_b=2, uint8_t ch_neg_b=3,
|
||||
uint16_t pwm_freq = 20000, // might not be hit exactly and may be rounded to nearby freq.
|
||||
uint8_t pwm_resolution = 12
|
||||
);
|
||||
|
||||
void begin();
|
||||
void enable();
|
||||
void disable();
|
||||
void set_field_angle(float angle_rad);
|
||||
float get_field_angle();
|
||||
void set_amplitude(float amplitude, bool immediate_update); // Input in range 0.0–1.0
|
||||
|
||||
private:
|
||||
void set_pwm(uint8_t ch_pos, uint8_t ch_neg, int32_t value);
|
||||
|
||||
uint8_t pin_en_a, pin_pos_a, pin_neg_a, pin_pwm_a;
|
||||
uint8_t pin_en_b, pin_pos_b, pin_neg_b, pin_pwm_b;
|
||||
uint8_t ch_pos_a, ch_neg_a;
|
||||
uint8_t ch_pos_b, ch_neg_b;
|
||||
|
||||
uint16_t pwm_freq;
|
||||
uint8_t pwm_resolution;
|
||||
uint16_t max_pwm;
|
||||
float amplitude; // scaled to 0–max_pwm
|
||||
float field_angle;
|
||||
};
|
||||
84
firmware/MotionControllerRP/src/hw_config.h
Normal file
84
firmware/MotionControllerRP/src/hw_config.h
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
// #define SINGLE_AXIS_BOARD
|
||||
#ifdef SINGLE_AXIS_BOARD
|
||||
// Single Axis Board
|
||||
#define PIN_BUILTIN_LED 16
|
||||
#define PIN_USER_BUTTON 24
|
||||
|
||||
#define PIN_M1_PWM_A_POS 13
|
||||
#define PIN_M1_PWM_A_NEG 12
|
||||
#define PIN_M1_PWM_B_POS 14
|
||||
#define PIN_M1_PWM_B_NEG 15
|
||||
|
||||
#define PIN_M2_PWM_A_POS 9
|
||||
#define PIN_M2_PWM_A_NEG 8
|
||||
#define PIN_M2_PWM_B_POS 10
|
||||
#define PIN_M2_PWM_B_NEG 11
|
||||
|
||||
#define PIN_M3_PWM_A_POS 5
|
||||
#define PIN_M3_PWM_A_NEG 4
|
||||
#define PIN_M3_PWM_B_POS 6
|
||||
#define PIN_M3_PWM_B_NEG 7
|
||||
|
||||
|
||||
#define PIN_MOTOR_EN 18
|
||||
#define PIN_MOTOR_PWMAB 19
|
||||
|
||||
#define PIN_ENCODER1_CS 20
|
||||
#define PIN_ENCODER2_CS 21
|
||||
#define PIN_ENCODER3_CS 22
|
||||
#define PIN_ENCODER_SCK 2
|
||||
#define PIN_ENCODER_MISO 0
|
||||
#define PIN_ENCODER_MOSI 3
|
||||
|
||||
#else
|
||||
// 3Axis Board
|
||||
#define PIN_BUILTIN_LED 23
|
||||
#define PIN_USER_BUTTON 24
|
||||
|
||||
#define PIN_M1_PWM_A_POS 13
|
||||
#define PIN_M1_PWM_A_NEG 12
|
||||
#define PIN_M1_PWM_B_POS 14
|
||||
#define PIN_M1_PWM_B_NEG 15
|
||||
|
||||
#define PIN_M2_PWM_A_POS 9
|
||||
#define PIN_M2_PWM_A_NEG 8
|
||||
#define PIN_M2_PWM_B_POS 10
|
||||
#define PIN_M2_PWM_B_NEG 11
|
||||
|
||||
#define PIN_M3_PWM_A_POS 5
|
||||
#define PIN_M3_PWM_A_NEG 4
|
||||
#define PIN_M3_PWM_B_POS 6
|
||||
#define PIN_M3_PWM_B_NEG 7
|
||||
|
||||
|
||||
#define PIN_MOTOR_EN 18
|
||||
#define PIN_MOTOR_PWMAB 19
|
||||
|
||||
#define PIN_ENCODER1_CS 20
|
||||
#define PIN_ENCODER2_CS 21
|
||||
#define PIN_ENCODER3_CS 22
|
||||
#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
|
||||
*/
|
||||
|
|
@ -0,0 +1 @@
|
|||
#include "kinematic_model_base.h"
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class Pose6DF;
|
||||
|
||||
//--- IKinemtaicModel -------------------------------------------------------------------
|
||||
|
||||
class IKinemtaicModel {
|
||||
public:
|
||||
virtual ~IKinemtaicModel() {};
|
||||
|
||||
// returns the number of joints
|
||||
virtual int get_joint_count();
|
||||
|
||||
// computes the end effector pose from joint positions
|
||||
virtual bool foreward(const float* joint_positions, Pose6DF& pose) = 0;
|
||||
|
||||
// computes joint positions from an end effector pose
|
||||
virtual bool inverse(const Pose6DF& pose, float* joint_positions) = 0;
|
||||
};
|
||||
|
|
@ -0,0 +1,288 @@
|
|||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "kinematic_model_delta3d.h"
|
||||
#include "utilities/math3d.h"
|
||||
#include "utilities/math_constants.h"
|
||||
#include "utilities/logging.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
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
|
||||
Vec3F base_offset(-32.5f, -32.5f, -32.5f);
|
||||
arm_length = 73.8f;
|
||||
rotor_radius = 15.0f;
|
||||
ee_attachment_points[0] = Vec3F(-0.5f, -14.5f, 2.0f);
|
||||
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
|
||||
/*
|
||||
Vec3F base_offset(-30.5f, -30.5f, -30.5f);
|
||||
arm_length = 2*36.5;
|
||||
rotor_radius = 15.0f;
|
||||
ee_attachment_points[0] = Vec3F(0.5f, -15.0f, 1.5f);
|
||||
ee_attachment_points[1] = Vec3F(1.5f, 0.5f, -15.0f);
|
||||
ee_attachment_points[2] = Vec3F(-15.0f, 1.5f, 0.5f);
|
||||
*/
|
||||
|
||||
// set transfomration 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(-42.0f, 0.5f, 32.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(32.0f, -42.0f, 0.5f)+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(0.5f, 32.0f, -42.0f)+base_offset;
|
||||
|
||||
rotor_angle_offset[0] = 46.2f*Constants::DEG2RAD;
|
||||
rotor_angle_offset[1] = 46.2f*Constants::DEG2RAD;
|
||||
rotor_angle_offset[2] = 46.2f*Constants::DEG2RAD;
|
||||
|
||||
for(int i=0; i<3; i++)
|
||||
base_to_actuator[i] = actuator_to_base[i].inverse();
|
||||
}
|
||||
|
||||
int KinematicModel_Delta3D::get_joint_count() {
|
||||
return 3;
|
||||
}
|
||||
|
||||
bool KinematicModel_Delta3D::foreward(const float* joint_positions, Pose6DF& pose) {
|
||||
Vec3F arm_attachment_points[3];
|
||||
for(int i=0; i<3; i++) {
|
||||
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
|
||||
// to find ee position. This only works if there is no ee rotation.
|
||||
arm_attachment_points[i] = p-ee_attachment_points[i];
|
||||
}
|
||||
|
||||
// compute three sphere intersection
|
||||
Vec3F intersections[2];
|
||||
bool ok = three_sphere_intersection(arm_attachment_points[0], arm_length,
|
||||
arm_attachment_points[1], arm_length,
|
||||
arm_attachment_points[2], arm_length,
|
||||
intersections);
|
||||
if(!ok) return false;
|
||||
|
||||
|
||||
// select correct solution
|
||||
Vec3F q = intersections[0].x > intersections[1].x ? intersections[0] : intersections[1];
|
||||
|
||||
pose.translation = q;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool KinematicModel_Delta3D::inverse(const Pose6DF& pose, float* joint_positions) {
|
||||
for(int i=0; i<3; i++) {
|
||||
// get ee attachment points in base coordinate system
|
||||
Vec3F intersections[2];
|
||||
Vec3F p = pose.transformPoint(ee_attachment_points[i]);
|
||||
// LOG_INFO(" p(base) = %f %f %f", p.x, p.y, p.z);
|
||||
|
||||
// transform attachment point to actuator coordinates
|
||||
p = base_to_actuator[i].transformPoint(p);
|
||||
|
||||
// compute intersection points
|
||||
bool ok = circle_sphere_intersection(rotor_radius, p, arm_length, intersections);
|
||||
if(!ok) return false;
|
||||
|
||||
// select correct solution based on x-position (in actuator coordinates)
|
||||
Vec3F q = intersections[0].x > intersections[1].x ? intersections[0] : intersections[1];
|
||||
|
||||
// compute joint angle
|
||||
float angle = -atan2(q.y, q.x); // joint angles are cw
|
||||
joint_positions[i] = rotor_angle_offset[i] + angle;
|
||||
}
|
||||
|
||||
// joint_positions[0] = pose.translation.x;
|
||||
// joint_positions[1] = pose.translation.y;
|
||||
// joint_positions[2] = pose.translation.z;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns the arm attachment point on the rotor for a given rotor angle
|
||||
Vec3F KinematicModel_Delta3D::arm_attachment_point(int joint_idx, float rotor_angle) {
|
||||
// Note: rotor angle is defined clockwise so 0 is the retracted state
|
||||
rotor_angle -= rotor_angle_offset[joint_idx];
|
||||
return Vec3F(cos(rotor_angle), -sin(rotor_angle), 0)*rotor_radius;
|
||||
}
|
||||
|
||||
void KinematicModel_Delta3D::test() {
|
||||
float D2R = Constants::DEG2RAD;
|
||||
|
||||
{
|
||||
LOG_INFO("\n# Foreward Kinematic");
|
||||
float joint_pos[3] = {45*D2R, 45*D2R, 45*D2R};
|
||||
Pose6DF pose;
|
||||
foreward(joint_pos, pose);
|
||||
Vec3F p = pose.translation;
|
||||
LOG_INFO("%f %f %f", p.x, p.y, p.z);
|
||||
}
|
||||
|
||||
{
|
||||
LOG_INFO("\n# Inverse Kinematic");
|
||||
Vec3F p(0.0, 0.0, 0.0);
|
||||
Pose6DF pose(p, QuaternionF());
|
||||
float joint_pos[3];
|
||||
inverse(pose, joint_pos);
|
||||
LOG_INFO("%f %f %f", joint_pos[0]/D2R, joint_pos[1]/D2R, joint_pos[2]/D2R);
|
||||
}
|
||||
|
||||
LOG_INFO("\n# Rotor Attachment Points");
|
||||
for(int i=0; i<3; i++) {
|
||||
for(float angle=0.0f; angle<90.0f; angle+=10.0f) {
|
||||
Vec3F p = arm_attachment_point(i, angle*Constants::DEG2RAD);
|
||||
p = actuator_to_base[i].transformPoint(p);
|
||||
LOG_INFO("%f %f %f", p.x, p.y, p.z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//*** FUNCTION **************************************************************************
|
||||
|
||||
/**
|
||||
* @brief Computes the intersection points between a circle in the XY-plane and a 3D sphere.
|
||||
*
|
||||
* Given:
|
||||
* - A circle centered at the origin (0,0,0) in the XY-plane with radius `r1`.
|
||||
* - A sphere centered at position `p` with radius `r2`.
|
||||
*
|
||||
* The function computes up to two 3D intersection points where the sphere intersects
|
||||
* the plane of the circle, and those points lie on the given circle.
|
||||
*
|
||||
* @param r1 Radius of the circle (must be >= 0).
|
||||
* @param p Center of the sphere (Vec3F: x, y, z).
|
||||
* @param r2 Radius of the sphere (must be >= 0).
|
||||
* @param intersections Output array of 2 Vec3F points. If there is an intersection,
|
||||
* both points are filled.
|
||||
*
|
||||
* @return true if there is at least one intersection point (either one or two),
|
||||
* false if there is no intersection.
|
||||
*/
|
||||
bool circle_sphere_intersection(double r1, const Vec3F& p, double r2, Vec3F intersections[2]) {
|
||||
const double px = p.x;
|
||||
const double py = p.y;
|
||||
const double pz = p.z;
|
||||
const double r2_sq = r2 * r2;
|
||||
const double pz_sq = pz * pz;
|
||||
|
||||
// Check if sphere intersects XY-plane and projected radius of sphere-circle in XY
|
||||
const double r_proj_sq = r2_sq - pz_sq;
|
||||
if (r_proj_sq < 0.0f)
|
||||
return false;
|
||||
const double r_proj = std::sqrt(r_proj_sq);
|
||||
|
||||
// Distance squared between circle centers
|
||||
const double d_sq = px * px + py * py;
|
||||
|
||||
// Check if circles intersect
|
||||
const double sum_r = r1 + r_proj;
|
||||
const double diff_r = std::abs(r1 - r_proj);
|
||||
if (d_sq > sum_r * sum_r || d_sq < diff_r * diff_r)
|
||||
return false;
|
||||
|
||||
// Distance between circle centers and its inverse
|
||||
const double d = std::sqrt(d_sq);
|
||||
const double inv_d = 1.0 / d;
|
||||
|
||||
// a = (r1^2 - r2^2 + d^2) / (2d)
|
||||
const double r1_sq = r1 * r1;
|
||||
const double a = (r1_sq - r_proj_sq + d_sq) * 0.5f * inv_d;
|
||||
|
||||
// h = sqrt(r1^2 - a^2)
|
||||
const double h_sq = r1_sq - a * a;
|
||||
if (h_sq < 1e-8f)
|
||||
return false; // numerical precision issue
|
||||
const double h = std::sqrt(h_sq);
|
||||
|
||||
// Base point (cx2, cy2)
|
||||
const double cx2 = px * (a * inv_d);
|
||||
const double cy2 = py * (a * inv_d);
|
||||
|
||||
// Offset vector
|
||||
const double rx = -py * (h * inv_d);
|
||||
const double ry = px * (h * inv_d);
|
||||
|
||||
intersections[0] = Vec3F(cx2 + rx, cy2 + ry, 0.0f);
|
||||
intersections[1] = Vec3F(cx2 - rx, cy2 - ry, 0.0f);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Computes the intersection points of three spheres in 3D space.
|
||||
*
|
||||
* Given three spheres defined by their centers (p1, p2, p3) and radii (r1, r2, r3),
|
||||
* this function computes up to two points where all three spheres intersect.
|
||||
* Returns false if no real intersection exists (e.g., spheres are too far apart or nearly tangent).
|
||||
*
|
||||
* @param p1 Center of the first sphere
|
||||
* @param r1 Radius of the first sphere
|
||||
* @param p2 Center of the second sphere
|
||||
* @param r2 Radius of the second sphere
|
||||
* @param p3 Center of the third sphere
|
||||
* @param r3 Radius of the third sphere
|
||||
* @param intersections Output array of two Vec3F points where the spheres intersect
|
||||
* @return true if a real intersection exists (two points), false otherwise
|
||||
*/
|
||||
bool three_sphere_intersection(const Vec3F& p1, float r1,
|
||||
const Vec3F& p2, float r2,
|
||||
const Vec3F& p3, float r3,
|
||||
Vec3F intersections[2])
|
||||
{
|
||||
const float eps = 1e-7f;
|
||||
const float r1_sqr = r1 * r1;
|
||||
|
||||
// Compute unit vector ex from p1 to p2
|
||||
Vec3F ex = p2 - p1;
|
||||
float d2 = ex.sqr_length();
|
||||
if (d2 < eps)
|
||||
return false;
|
||||
|
||||
float d = std::sqrt(d2);
|
||||
float inv_d = 1.0f / d;
|
||||
ex = ex * inv_d;
|
||||
|
||||
// Project p3 onto ex to compute scalar i
|
||||
Vec3F temp = p3 - p1;
|
||||
float i = ex.dot(temp);
|
||||
|
||||
// Compute unit vector ey perpendicular to ex
|
||||
Vec3F ey = temp - ex * i;
|
||||
float ey2 = ey.sqr_length();
|
||||
if (ey2 < eps)
|
||||
return false;
|
||||
|
||||
float inv_ey = 1.0f / std::sqrt(ey2);
|
||||
ey = ey * inv_ey;
|
||||
float j = ey.dot(temp);
|
||||
|
||||
// Compute unit vector ez orthogonal to ex and ey
|
||||
Vec3F ez = ex.cross(ey);
|
||||
|
||||
// Compute x and y coordinates in ex/ey plane
|
||||
float x = (r1_sqr - r2 * r2 + d * d) * 0.5f * inv_d;
|
||||
float y = (r1_sqr - r3 * r3 + i * i + j * j - 2.0f * i * x) * 0.5f * inv_ey;
|
||||
|
||||
// Compute z coordinate along ez axis
|
||||
float z2 = r1_sqr - x * x - y * y;
|
||||
if (z2 < eps) return false; // no real solution
|
||||
float z = std::sqrt(z2);
|
||||
|
||||
// Compute the two possible intersection points
|
||||
Vec3F base = p1 + ex * x + ey * y;
|
||||
intersections[0] = base + ez * z;
|
||||
intersections[1] = base - ez * z;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#pragma once
|
||||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "kinematic_model_base.h"
|
||||
#include "utilities/math3d.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class Pose6DF;
|
||||
|
||||
//--- KinemtaicModel_Delta3D ------------------------------------------------------------
|
||||
|
||||
class KinematicModel_Delta3D : public IKinemtaicModel {
|
||||
public:
|
||||
KinematicModel_Delta3D();
|
||||
|
||||
int get_joint_count();
|
||||
bool foreward(const float* joint_positions, Pose6DF& pose) override;
|
||||
bool inverse(const Pose6DF& pose, float* joint_positions) override;
|
||||
|
||||
void test();
|
||||
|
||||
protected:
|
||||
Vec3F arm_attachment_point(int joint_idx, float rotor_angle);
|
||||
|
||||
public:
|
||||
float arm_length;
|
||||
float rotor_radius;
|
||||
float rotor_angle_offset[3]; // angle offset of the neutral position from zero position in rad
|
||||
Pose6DF base_to_actuator[3];
|
||||
Pose6DF actuator_to_base[3];
|
||||
Vec3F ee_attachment_points[3]; // EE arm attachment points in endeffector coordinate system
|
||||
};
|
||||
|
||||
//*** FUNCTION **************************************************************************
|
||||
|
||||
bool circle_sphere_intersection(double r1, const Vec3F& p, double r2, Vec3F intersections[2]);
|
||||
bool three_sphere_intersection(const Vec3F& p1, float r1,
|
||||
const Vec3F& p2, float r2,
|
||||
const Vec3F& p3, float r3,
|
||||
Vec3F intersections[2]);
|
||||
325
firmware/MotionControllerRP/src/main.cpp
Normal file
325
firmware/MotionControllerRP/src/main.cpp
Normal file
|
|
@ -0,0 +1,325 @@
|
|||
#include "main.h"
|
||||
|
||||
#include "hardware/clocks.h"
|
||||
#include "hardware/pll.h"
|
||||
#include "hardware/vreg.h"
|
||||
|
||||
#include <NeoPixelConnect.h>
|
||||
#include <Wire.h>
|
||||
#include <algorithm>
|
||||
|
||||
#include "robot.h"
|
||||
#include "utilities/logging.h"
|
||||
#include "utilities/frequency_counter.h"
|
||||
#include "kinemtaic_models/kinematic_model_delta3d.h"
|
||||
|
||||
#include "hw_config.h"
|
||||
|
||||
//*** GLOBALS ***************************************************************************
|
||||
|
||||
NeoPixelConnect strip(PIN_BUILTIN_LED, 1);
|
||||
Robot robot(0.01f);
|
||||
|
||||
/*
|
||||
MT6835Encoder encoder1(spi0, PIN_ENCODER1_CS);
|
||||
MT6835Encoder encoder2(spi0, PIN_ENCODER2_CS);
|
||||
MT6835Encoder encoder3(spi0, PIN_ENCODER3_CS);
|
||||
|
||||
TB6612MotorDriver motor_driver1(
|
||||
PIN_MOTOR_EN, PIN_M1_PWM_A_POS, PIN_M1_PWM_A_NEG, PIN_MOTOR_PWMAB,
|
||||
PIN_MOTOR_EN, PIN_M1_PWM_B_POS, PIN_M1_PWM_B_NEG, PIN_MOTOR_PWMAB
|
||||
);
|
||||
TB6612MotorDriver motor_driver2(
|
||||
PIN_MOTOR_EN, PIN_M2_PWM_A_POS, PIN_M2_PWM_A_NEG, PIN_MOTOR_PWMAB,
|
||||
PIN_MOTOR_EN, PIN_M2_PWM_B_POS, PIN_M2_PWM_B_NEG, PIN_MOTOR_PWMAB
|
||||
);
|
||||
TB6612MotorDriver motor_driver3(
|
||||
PIN_MOTOR_EN, PIN_M3_PWM_A_POS, PIN_M3_PWM_A_NEG, PIN_MOTOR_PWMAB,
|
||||
PIN_MOTOR_EN, PIN_M3_PWM_B_POS, PIN_M3_PWM_B_NEG, PIN_MOTOR_PWMAB
|
||||
);
|
||||
|
||||
ServoController servo_controller1(motor_driver1, encoder1, 400/4);
|
||||
ServoController servo_controller2(motor_driver2, encoder2, 400/4);
|
||||
ServoController servo_controller3(motor_driver3, encoder3, 400/4);
|
||||
FrequencyCounter loop_freq_counter(1000);
|
||||
|
||||
PathPlanner planner(0.01f);
|
||||
MotionController motion_controller(&planner);
|
||||
Pose6DF current_pose;
|
||||
|
||||
CommandParser command_parser; */
|
||||
|
||||
//*** FUNCTIONS *************************************************************************
|
||||
|
||||
// Run before setup()
|
||||
//__attribute__((constructor))
|
||||
void overclock() {
|
||||
vreg_set_voltage(VREG_VOLTAGE_1_20); // For >133 MHz
|
||||
busy_wait_us(10 * 1000); // 10 ms delay
|
||||
set_sys_clock_khz(250000, true); // Set to 250 MHz
|
||||
}
|
||||
|
||||
void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
|
||||
strip.neoPixelSetValue(0, r, g, b, false);
|
||||
delayMicroseconds(1000);
|
||||
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);
|
||||
sleep_ms(period_time_ms/2);
|
||||
set_led_color(0, 0, 0);
|
||||
sleep_ms(period_time_ms/2);
|
||||
}
|
||||
}
|
||||
|
||||
void main_core0() {
|
||||
uint64_t last_time = time_us_64();
|
||||
|
||||
while(true) {
|
||||
// update motion controller
|
||||
robot.update_command_parser();
|
||||
robot.update_path_planner();
|
||||
}
|
||||
}
|
||||
|
||||
void main_core1() {
|
||||
LOG_INFO("starting servo controll loops on core 1...");
|
||||
|
||||
uint64_t last_time = time_us_64();
|
||||
while(true) {
|
||||
// get time and detla time
|
||||
uint64_t time_us = time_us_64();
|
||||
float dt = float(time_us - last_time)*1e-6f;
|
||||
last_time = time_us;
|
||||
|
||||
robot.update_servo_controllers(dt);
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
led_blink(0, 20, 0, 1, 4000/3);
|
||||
// stdio_init_all(); // Initializes USB or UART stdio
|
||||
overclock();
|
||||
// Serial.begin(921600);
|
||||
Logger::instance().begin(921600, false);
|
||||
while(!Serial);
|
||||
|
||||
set_led_color(50, 10, 0);
|
||||
// auto* test = new KinematicModel_Delta3D(); test->test(); delete test;
|
||||
|
||||
delay(100); // Allow time for serial monitor to connect
|
||||
Serial.printf("System clock: %i Mhz\n", int32_t(clock_get_hz(clk_sys))/1000/1000);
|
||||
|
||||
LOG_INFO("initializing robot...");
|
||||
robot.init();
|
||||
|
||||
LOG_INFO("homing axes...");
|
||||
robot.home();
|
||||
|
||||
multicore_launch_core1(&main_core1);
|
||||
|
||||
set_led_color(0, 10, 0);
|
||||
LOG_INFO("initialization finished...");
|
||||
|
||||
return;
|
||||
|
||||
/*
|
||||
rotencoder_wire.setSDA(PIN_ENCODER_SDA);
|
||||
rotencoder_wire.setSCL(PIN_ENCODER_SCL);
|
||||
rotencoder_wire.begin();
|
||||
rotencoder_wire.setClock(1000000);
|
||||
encoder.init();
|
||||
encoder.set_hysteresis(0x4); //0x6);
|
||||
*/
|
||||
/* pinMode(PIN_USER_BUTTON, INPUT_PULLUP);
|
||||
|
||||
// init encoders
|
||||
MT6835Encoder::setup_spi(spi0, PIN_ENCODER_SCK, PIN_ENCODER_MOSI, PIN_ENCODER_MISO, 8000000);
|
||||
encoder1.init(0x5, 0x4);
|
||||
encoder2.init(0x5, 0x4);
|
||||
encoder3.init(0x5, 0x4);
|
||||
|
||||
servo_controller1.init(0.5);
|
||||
servo_controller2.init(0.5);
|
||||
servo_controller3.init(0.5);
|
||||
|
||||
delay(1000);
|
||||
|
||||
set_led_color(50, 10, 0);
|
||||
|
||||
servo_controller1.home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
|
||||
calibrate_actuator(servo_controller1);
|
||||
|
||||
servo_controller2.home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
|
||||
calibrate_actuator(servo_controller2);
|
||||
|
||||
servo_controller3.home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
|
||||
calibrate_actuator(servo_controller3);
|
||||
|
||||
set_led_color(0, 10, 0);
|
||||
*/
|
||||
}
|
||||
|
||||
void loop() {
|
||||
main_core0();
|
||||
}
|
||||
|
||||
/*
|
||||
int it=0;
|
||||
float target_angle1 = 50.0f/180.0f*PI;
|
||||
float target_angle2 = 50.0f/180.0f*PI;
|
||||
float target_angle3 = 50.0f/180.0f*PI;
|
||||
int k=0;
|
||||
uint64_t last_time = time_us_64();
|
||||
uint64_t last_print_time = time_us_64();
|
||||
|
||||
|
||||
void loop_encoder_test() {
|
||||
float encoder_angle = encoder3.read_abs_angle();
|
||||
Serial.printf(">angle: %f\n", encoder_angle*360/TWO_PI);
|
||||
delay(10);
|
||||
}
|
||||
|
||||
void loop_motor_test() {
|
||||
uint64_t time_us = time_us_64();
|
||||
//target_angle = (30.0f+0.07f*((time_us>>19)%2))*DEG_TO_RAD;
|
||||
float target_angle = (1000.05f*sin(float(time_us)*3e-5f))/180*PI;
|
||||
motor_driver1.set_field_angle(target_angle);
|
||||
|
||||
target_angle = (1000.05f*sin(float(time_us)*4e-5f))/180*PI;
|
||||
motor_driver2.set_field_angle(target_angle);
|
||||
|
||||
target_angle = (1000.05f*sin(float(time_us)*5e-5f))/180*PI;
|
||||
motor_driver3.set_field_angle(target_angle);
|
||||
}
|
||||
|
||||
void loop_old() {
|
||||
// return;
|
||||
//loop_motor_test(); return;
|
||||
// loop_encoder_test(); return;
|
||||
|
||||
// get time and detla time
|
||||
uint64_t time_us = time_us_64();
|
||||
float dt = float(time_us - last_time)*1e-6f;
|
||||
float one_over_dt = 1.0f/dt;
|
||||
last_time = time_us;
|
||||
|
||||
servo_controller1.update(shared_data.joint_positions[0], dt, one_over_dt);
|
||||
servo_controller2.update(shared_data.joint_positions[1], dt, one_over_dt);
|
||||
servo_controller3.update(shared_data.joint_positions[2], dt, one_over_dt);
|
||||
loop_freq_counter.update(dt);
|
||||
|
||||
// motor_servo_update(dt_ms);
|
||||
|
||||
|
||||
// print info
|
||||
if(time_us-last_print_time > 10000 && true) {
|
||||
//Serial.printf(">angle: %f\n", encoder_angle*360/TWO_PI);
|
||||
//Serial.printf(">field: %f\n", field_angle*360/TWO_PI);
|
||||
Serial.printf(">pos_error [µrad]: %f\n", servo_controller3.get_position_error()*1e6f);
|
||||
Serial.printf(">output [deg]: %f\n", servo_controller3.output*float(RAD_TO_DEG));
|
||||
Serial.printf(">motor_pos [deg]: %f\n", servo_controller3.get_position()*float(RAD_TO_DEG));
|
||||
//Serial.printf(">motor_pos: %f µm\n", servo_controller.get_position()*15.0e6f);
|
||||
//Serial.printf(">update_khz: %f\n", float(loop_freq_counter.get())*0.001);
|
||||
|
||||
//Serial.printf(">e: %f\n",e*360/TWO_PI);
|
||||
|
||||
last_print_time = time_us;
|
||||
}
|
||||
|
||||
//target_angle2 = (50.0f+0.012f*((time_us>>20)%2))*DEG_TO_RAD;
|
||||
//target_angle3 = (50.0f+0.012f*(1-(time_us>>20)%2))*DEG_TO_RAD;
|
||||
|
||||
//target_angle2 = (50.0f+0.0002f*sin(float(time_us)*5e-6f))/180*PI;
|
||||
//target_angle3 = (50.0f+0.0002f*cos(float(time_us)*5e-6f))/180*PI;
|
||||
|
||||
// target_angle1 = (50.0f+20.001f*trapezoidal_wave(float(time_us)*5e-6f-PI*0.33f))/180*PI;
|
||||
// target_angle2 = (50.0f+20.001f*trapezoidal_wave(float(time_us)*5e-6f))/180*PI;
|
||||
// target_angle3 = (50.0f+20.001f*trapezoidal_wave(float(time_us)*5e-6f+PI*0.33f))/180*PI;
|
||||
|
||||
//target_angle = (50.0f+20.005f*triangle_wave(float(time_us)*1.0e-5f))/180*PI;
|
||||
|
||||
|
||||
return;
|
||||
/*
|
||||
it++;
|
||||
|
||||
float ki = 0.0f;
|
||||
if(k<5000 || digitalRead(PIN_USER_BUTTON) == 0)
|
||||
ki = 0.3f;
|
||||
k++;
|
||||
|
||||
// test
|
||||
float max_integral = 0.15f;
|
||||
float e = (target_angle - encoder_angle);
|
||||
//float gain_scale = e<0.25f/360*TWO_PI ? 3.0 : 1.0f;
|
||||
//float d = (e-prev_e) * 10.0f;
|
||||
//d_filtered = d_filtered*0.5f + d*0.5f;
|
||||
|
||||
float velocity_setpoint = 0.2f * e;
|
||||
float velocity = encoder_angle-prev_angle;
|
||||
float velocity_error = velocity_setpoint - velocity;
|
||||
|
||||
integral += std::clamp(1.25f * velocity_error, -max_integral, max_integral); // velocity PI
|
||||
|
||||
// float ig = max(min(e*ki, max_integral), -max_integral);
|
||||
// integral += ig - v*0.4f;
|
||||
|
||||
out = integral;// + std::clamp(e*0.0f, -3.415926f*0.05f, 3.415926f*0.05f);// - d_filtered;
|
||||
//float p = max(min(e*3, 1), -1);
|
||||
motor.set_field_angle(target_angle*e2m_scale+out);
|
||||
//motor.set_field_angle(out);
|
||||
prev_e = e;
|
||||
prev_angle = encoder_angle;
|
||||
|
||||
if(it>100) {
|
||||
// Serial.print(rawAngle);
|
||||
Serial.print(">angle: ");
|
||||
// float angleDegrees = angle_tracker. * 360.0f / 16384.0f;
|
||||
Serial.println(encoder_angle*360/TWO_PI, 9);
|
||||
Serial.print(">out: ");
|
||||
Serial.println(out, 9);
|
||||
|
||||
Serial.printf(">e: %f\n",e*360/TWO_PI);
|
||||
// Serial.printf(">v: %f\n",velocity);
|
||||
|
||||
if(fabs(target_angle-encoder_angle)*360/TWO_PI > 0.05)
|
||||
strip.neoPixelSetValue(0, 10, 0, 0, false);
|
||||
else
|
||||
strip.neoPixelSetValue(0, 0, 10, 0, false);
|
||||
|
||||
delayMicroseconds(10);
|
||||
strip.neoPixelShow();
|
||||
}
|
||||
|
||||
// target_angle = (60.0f+20.00f*((k/1000)%2))/180*PI;
|
||||
// target_angle = (60.0f+0.1f*sin(float(k)*0.01))/180*PI;
|
||||
// k++;
|
||||
|
||||
// int32_t output = pos_controller.compute(pos_controller.to_fixpoint(target_angle),
|
||||
// pos_controller.to_fixpoint(angle));
|
||||
|
||||
// a = pos_controller.from_fixpoint(output);
|
||||
//a += 0.001f;
|
||||
// motor.set_field_angle(a);
|
||||
|
||||
// float ma = (angle)*e2m_scale + e2m_offset;
|
||||
// motor.set_field_angle(ma);
|
||||
|
||||
/* static uint16_t hue = 0; // 0-255 for full RGB cycle
|
||||
uint8_t r, g, b;
|
||||
//hsv2rgb(hue, 255, 1, r, g, b); // 50 = brightness (0-255)
|
||||
r = 0; g=0; b=0;
|
||||
g = raw_angle%2 == 0 ? 10 : 0;
|
||||
|
||||
// Set the single pixel to the current color
|
||||
strip.neoPixelSetValue(0, r, g, b, false);
|
||||
strip.neoPixelShow();
|
||||
|
||||
hue = (hue + 1) % 256; // Adjust increment for speed
|
||||
delay(20); // Adjust delay for smoothness
|
||||
*/
|
||||
// strip.neoPixelSetValue(0, 2, 190, 3, false);
|
||||
// }
|
||||
0
firmware/MotionControllerRP/src/main.h
Normal file
0
firmware/MotionControllerRP/src/main.h
Normal file
|
|
@ -0,0 +1,40 @@
|
|||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "motion_controller.h"
|
||||
#include "path_planner.h"
|
||||
#include "utilities/logging.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
MotionController::MotionController(PathPlanner* path_planner) {
|
||||
MotionController::path_planner = path_planner;
|
||||
current_time = 0.0f;
|
||||
}
|
||||
|
||||
bool MotionController::update(float dt, float* joint_positions, float* joint_velocities) {
|
||||
// increment time counter
|
||||
current_time += dt;
|
||||
|
||||
// check if end of current path segment exceeded and if so, fetch next one
|
||||
float segment_duration = current_path_segment.get_duration();
|
||||
while(current_time > segment_duration) {
|
||||
// get next path segment from queue
|
||||
bool queue_empty = !path_planner->pop_js_path_segment(current_path_segment);
|
||||
if(queue_empty) {
|
||||
current_time = segment_duration;
|
||||
break;
|
||||
}
|
||||
|
||||
// update current time and segment duration
|
||||
current_time -= segment_duration;
|
||||
segment_duration = current_path_segment.get_duration();
|
||||
}
|
||||
|
||||
if(!current_path_segment.is_initialized())
|
||||
return false;
|
||||
|
||||
// evaluate path segment
|
||||
current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#pragma once
|
||||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "path_segment.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class PathPlanner;
|
||||
|
||||
//--- MotionController ------------------------------------------------------------------
|
||||
|
||||
class MotionController {
|
||||
public:
|
||||
MotionController(PathPlanner* path_planner);
|
||||
|
||||
// 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);
|
||||
|
||||
private:
|
||||
PathPlanner* path_planner;
|
||||
|
||||
float current_time;
|
||||
JointSpacePathSegment current_path_segment;
|
||||
};
|
||||
101
firmware/MotionControllerRP/src/motion_control/path_planner.cpp
Normal file
101
firmware/MotionControllerRP/src/motion_control/path_planner.cpp
Normal file
|
|
@ -0,0 +1,101 @@
|
|||
|
||||
#include "hardware/sync.h"
|
||||
|
||||
#include "path_planner.h"
|
||||
#include "utilities/logging.h"
|
||||
|
||||
PathPlanner::PathPlanner(IKinemtaicModel* kinematic_model, float time_step) {
|
||||
segment_time_step = time_step;
|
||||
kinematic_model = kinematic_model;
|
||||
}
|
||||
|
||||
PathPlanner::~PathPlanner() {
|
||||
}
|
||||
|
||||
void PathPlanner::set_kinematic_model(IKinemtaicModel* kinematic_model) {
|
||||
PathPlanner::kinematic_model = kinematic_model;
|
||||
}
|
||||
|
||||
bool PathPlanner::add_cartesian_path_segment(const CartesianPathSegment& path_segment) {
|
||||
auto* new_segment = ct_path_segment_queue.push(path_segment);
|
||||
if(new_segment == nullptr) {
|
||||
// queue full
|
||||
return false;
|
||||
}
|
||||
|
||||
// TODO: do look ahead planning of queue
|
||||
new_segment->compute_motion_profile(); // for testing
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PathPlanner::process(bool disable_interrupts_for_queue_update) {
|
||||
// create new segment generator for next cartesian path segment
|
||||
// the segment stays in the queue until it is completed
|
||||
if(segment_generator == nullptr && ct_path_segment_queue.empty() == false) {
|
||||
auto* current_segment = ct_path_segment_queue.peek();
|
||||
segment_generator = new JointSpacePathSegmentGenerator(current_segment,
|
||||
kinematic_model,
|
||||
segment_time_step);
|
||||
/*LOG_INFO("Starting segment: duration=%fs, (%f, %f, %f)->(%f, %f, %f) | queue size: %i",
|
||||
current_segment->get_duration(),
|
||||
current_segment->start_pose.translation.x,
|
||||
current_segment->start_pose.translation.y,
|
||||
current_segment->start_pose.translation.z,
|
||||
current_segment->end_pose.translation.x,
|
||||
current_segment->end_pose.translation.y,
|
||||
current_segment->end_pose.translation.z,
|
||||
ct_path_segment_queue.size()); */
|
||||
}
|
||||
|
||||
// generate joint space segment
|
||||
if(segment_generator != nullptr && js_path_segment_queue.full() == false) {
|
||||
JointSpacePathSegment segment;
|
||||
bool end_reached = segment_generator->generate_next(segment);
|
||||
|
||||
// update output queue
|
||||
if(disable_interrupts_for_queue_update) {
|
||||
uint32_t status = save_and_disable_interrupts();
|
||||
js_path_segment_queue.push(segment);
|
||||
restore_interrupts(status);
|
||||
} else {
|
||||
js_path_segment_queue.push(segment);
|
||||
}
|
||||
|
||||
// LOG_INFO("Adding joint space segment: [%f, %f, %f] -> [%f, %f, %f]",
|
||||
// segment.start_pos[0], segment.start_pos[1], segment.start_pos[2],
|
||||
// segment.end_pos[0], segment.end_pos[1], segment.end_pos[2]);
|
||||
|
||||
// check if current cartesian path segmetn is completed
|
||||
if(end_reached) {
|
||||
// remove current cartesian path segment from ringbuffer
|
||||
ct_path_segment_queue.pop();
|
||||
// destroy segment generator
|
||||
delete segment_generator;
|
||||
segment_generator = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* retrieve
|
||||
*/
|
||||
bool PathPlanner::pop_js_path_segment(JointSpacePathSegment& segment) {
|
||||
return js_path_segment_queue.pop(segment);
|
||||
}
|
||||
|
||||
bool PathPlanner::all_finished() {
|
||||
return js_path_segment_queue.empty() && ct_path_segment_queue.empty() && segment_generator == nullptr;
|
||||
}
|
||||
|
||||
int PathPlanner::input_queue_full() {
|
||||
return ct_path_segment_queue.full();
|
||||
}
|
||||
|
||||
int PathPlanner::input_queue_size() {
|
||||
return ct_path_segment_queue.size();
|
||||
}
|
||||
|
||||
void PathPlanner::run_look_ahead_planning() {
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
#pragma once
|
||||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "path_segment.h"
|
||||
#include "utilities/ringbuffer.h"
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class IKinemtaicModel;
|
||||
|
||||
//--- PathPlanner -----------------------------------------------------------------------
|
||||
|
||||
class PathPlanner {
|
||||
public:
|
||||
static constexpr int CT_QUEUE_SIZE = 64;
|
||||
static constexpr int JS_QUEUE_SIZE = 32;
|
||||
|
||||
public:
|
||||
PathPlanner(IKinemtaicModel* kinematic_model, float time_step);
|
||||
~PathPlanner();
|
||||
|
||||
// sets the kinematic model for foreward and inverse kinematic calculations
|
||||
void set_kinematic_model(IKinemtaicModel* kinematic_model);
|
||||
|
||||
// adds a new cartesian space path segment to the planner queue
|
||||
bool add_cartesian_path_segment(const CartesianPathSegment& path_segment);
|
||||
|
||||
// Retrieves the next joint space path segment from the queue, returns false
|
||||
// if queue is empty.
|
||||
bool pop_js_path_segment(JointSpacePathSegment& segment);
|
||||
|
||||
// Processes the queued cartesian path segments and generates one
|
||||
// joint space path segment if possible. Call this repeatedly.
|
||||
void process(bool disable_interrupts_for_queue_update);
|
||||
|
||||
// returns true if all ques are empty and if everything is finished
|
||||
bool all_finished();
|
||||
|
||||
// returns the number of free items in the input queue
|
||||
int input_queue_full();
|
||||
|
||||
// returns the current number of queued items
|
||||
int input_queue_size();
|
||||
|
||||
private:
|
||||
void run_look_ahead_planning();
|
||||
|
||||
private:
|
||||
RingBuffer<CartesianPathSegment, CT_QUEUE_SIZE> ct_path_segment_queue;
|
||||
RingBuffer<JointSpacePathSegment, JS_QUEUE_SIZE> js_path_segment_queue;
|
||||
|
||||
IKinemtaicModel* kinematic_model;
|
||||
JointSpacePathSegmentGenerator* segment_generator = nullptr;
|
||||
float segment_time_step;
|
||||
};
|
||||
|
||||
282
firmware/MotionControllerRP/src/motion_control/path_segment.cpp
Normal file
282
firmware/MotionControllerRP/src/motion_control/path_segment.cpp
Normal file
|
|
@ -0,0 +1,282 @@
|
|||
#include "path_segment.h"
|
||||
#include "utilities/logging.h"
|
||||
#include "kinemtaic_models/kinematic_model_base.h"
|
||||
|
||||
//--- MotionProfileConstAcc -------------------------------------------------------------
|
||||
|
||||
MotionProfileConstAcc::MotionProfileConstAcc(float dwell_time) {
|
||||
MotionProfileConstAcc::t1 = 0.0f;
|
||||
MotionProfileConstAcc::t2 = dwell_time;
|
||||
MotionProfileConstAcc::t3 = dwell_time;
|
||||
MotionProfileConstAcc::d1 = 0.0f;
|
||||
MotionProfileConstAcc::d2 = 1.0f;
|
||||
MotionProfileConstAcc::v_peak = 0.0f;
|
||||
MotionProfileConstAcc::acceleration = 0.0f;
|
||||
}
|
||||
|
||||
MotionProfileConstAcc::MotionProfileConstAcc(
|
||||
float distance,
|
||||
float v_start,
|
||||
float v_end,
|
||||
float max_velocity,
|
||||
float max_acceleration)
|
||||
{
|
||||
MotionProfileConstAcc::acceleration = max_acceleration;
|
||||
MotionProfileConstAcc::v_start = v_start;
|
||||
MotionProfileConstAcc::v_end = v_end;
|
||||
|
||||
if (distance <= 1e-7f) {
|
||||
MotionProfileConstAcc::t1 = 0.0f;
|
||||
MotionProfileConstAcc::t2 = 0.0f;
|
||||
MotionProfileConstAcc::t3 = 0.0f;
|
||||
MotionProfileConstAcc::d1 = 0.0f;
|
||||
MotionProfileConstAcc::d2 = 1.0f;
|
||||
MotionProfileConstAcc::v_peak = 0.0f;
|
||||
MotionProfileConstAcc::acceleration = 0.0f;
|
||||
} else {
|
||||
const float inv_max_acceleration = 1.0f / max_acceleration;
|
||||
|
||||
// Time to accelerate/decelerate, using multiplication by inverse accel
|
||||
float t_accel = (max_velocity - v_start) * inv_max_acceleration;
|
||||
float t_decel = (max_velocity - v_end) * inv_max_acceleration;
|
||||
|
||||
// Distances covered during accel/decel
|
||||
float d_accel = 0.5f * (v_start + max_velocity) * t_accel;
|
||||
float d_decel = 0.5f * (max_velocity + v_end) * t_decel;
|
||||
|
||||
float d_cruise = distance - (d_accel + d_decel);
|
||||
|
||||
if (d_cruise >= 0.0f) {
|
||||
// Trapezoidal velocity profile
|
||||
MotionProfileConstAcc::t1 = t_accel;
|
||||
MotionProfileConstAcc::t2 = t1 + d_cruise / max_velocity;
|
||||
MotionProfileConstAcc::t3 = t2 + t_decel;
|
||||
MotionProfileConstAcc::v_peak = max_velocity;
|
||||
} else {
|
||||
// Triangular velocity profile: recompute peak velocity v_peak
|
||||
float v_peak_sq = max_acceleration * distance + 0.5f * (v_start * v_start + v_end * v_end);
|
||||
float v_peak = std::sqrt(std::max(0.0f, v_peak_sq));
|
||||
|
||||
MotionProfileConstAcc::t1 = (v_peak - v_start) * inv_max_acceleration;
|
||||
MotionProfileConstAcc::t2 = t1 + 0.0f;
|
||||
MotionProfileConstAcc::t3 = t2 + (v_peak - v_end) * inv_max_acceleration;
|
||||
MotionProfileConstAcc::v_peak = v_peak;
|
||||
}
|
||||
}
|
||||
|
||||
// normalize velocity and acceleration to interpolator range (0..1)
|
||||
const float inv_distance = 1.0f/distance;
|
||||
v_start *= inv_distance;
|
||||
v_end *= inv_distance;
|
||||
v_peak *= inv_distance;
|
||||
acceleration *= inv_distance;
|
||||
|
||||
// precompute some values for faster evaluation
|
||||
MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
|
||||
MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
|
||||
|
||||
//LOG_INFO("d1=%f, d2=%f, d3=%f", d1, d2, distance);
|
||||
//LOG_INFO("t1=%f, t2=%f, t3=%f", t1, t2, t3);
|
||||
}
|
||||
|
||||
float MotionProfileConstAcc::evaluate(float time) const {
|
||||
if (time <= 0.0f) {
|
||||
return 0.0f;
|
||||
} else if (time < t1) {
|
||||
// Acceleration phase
|
||||
return v_start * time + 0.5f * acceleration * time * time;
|
||||
} else if (time < t2) {
|
||||
// Cruise phase
|
||||
float dt = time - t1;
|
||||
return d1 + v_peak * dt;
|
||||
} else if (time < t3) {
|
||||
// Deceleration phase
|
||||
float dt = time - t2;
|
||||
return d2 + v_peak * dt - 0.5f * acceleration * dt * dt;
|
||||
} else {
|
||||
// Finished
|
||||
return 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
//--- CartesianPathSegment --------------------------------------------------------------
|
||||
|
||||
CartesianPathSegment::CartesianPathSegment() {
|
||||
dwell_time = 0.0f;
|
||||
}
|
||||
|
||||
CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
|
||||
const Pose6DF& end_pose,
|
||||
const LinearAngular& target_velocity,
|
||||
const LinearAngular& max_acceleration)
|
||||
{
|
||||
CartesianPathSegment::dwell_time = 0.0f;
|
||||
CartesianPathSegment::start_pose = start_pose;
|
||||
CartesianPathSegment::end_pose = end_pose;
|
||||
|
||||
CartesianPathSegment::target_velocity = target_velocity;
|
||||
CartesianPathSegment::start_velocity = LinearAngular(0.0f, 0.0f);
|
||||
CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
|
||||
CartesianPathSegment::max_acceleration = max_acceleration;
|
||||
|
||||
travel_distance.linear = (end_pose.translation - start_pose.translation).length();
|
||||
travel_distance.angular = (start_pose.rotation.normalized_inverse() * end_pose.rotation).angle();
|
||||
}
|
||||
|
||||
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
|
||||
{
|
||||
CartesianPathSegment::dwell_time = dwell_time;
|
||||
CartesianPathSegment::start_pose = pose;
|
||||
CartesianPathSegment::end_pose = pose;
|
||||
|
||||
CartesianPathSegment::target_velocity = 0.0f;
|
||||
CartesianPathSegment::start_velocity = LinearAngular(0.0f, 0.0f);
|
||||
CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
|
||||
CartesianPathSegment::max_acceleration = 0.0f;
|
||||
|
||||
travel_distance.linear = 0.0f;
|
||||
travel_distance.angular = 0.0f;
|
||||
}
|
||||
|
||||
|
||||
void CartesianPathSegment::compute_motion_profile() {
|
||||
// LOG_INFO("compute_motion_profile...");
|
||||
if(dwell_time > 0.0f) {
|
||||
motion_profile = MotionProfileConstAcc(dwell_time);
|
||||
} else {
|
||||
MotionProfileConstAcc linear_profile(travel_distance.linear,start_velocity.linear,
|
||||
end_velocity.linear, target_velocity.linear,
|
||||
max_acceleration.linear);
|
||||
|
||||
MotionProfileConstAcc angular_profile(travel_distance.angular, start_velocity.angular,
|
||||
end_velocity.angular, target_velocity.angular,
|
||||
max_acceleration.angular);
|
||||
|
||||
// select profile that requires the longest time
|
||||
if(linear_profile.t3 > angular_profile.t3) {
|
||||
motion_profile = linear_profile;
|
||||
} else {
|
||||
motion_profile = angular_profile;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CartesianPathSegment::evaluate(float time, Pose6DF& pose) const {
|
||||
// evaluate motion profile
|
||||
float t = motion_profile.evaluate(time);
|
||||
|
||||
// interpolate pose
|
||||
pose = Pose6DF::lerp(start_pose, end_pose, t);
|
||||
}
|
||||
|
||||
float CartesianPathSegment::get_duration() const {
|
||||
return motion_profile.t3;
|
||||
}
|
||||
|
||||
//--- JointSpacePathSegment -------------------------------------------------------------
|
||||
|
||||
JointSpacePathSegment::JointSpacePathSegment() {
|
||||
for(int i=0; i<NUM_JOINTS; i++) {
|
||||
JointSpacePathSegment::start_pos[i] = 0.0f;
|
||||
JointSpacePathSegment::end_pos[i] = 0.0f;
|
||||
}
|
||||
duration = 0.0f;
|
||||
inv_duration = 0.0f;
|
||||
initialized = false;
|
||||
}
|
||||
|
||||
JointSpacePathSegment::JointSpacePathSegment(
|
||||
const float start_pos[NUM_JOINTS],
|
||||
const float end_pos[NUM_JOINTS],
|
||||
float duration)
|
||||
{
|
||||
JointSpacePathSegment::duration = duration;
|
||||
JointSpacePathSegment::inv_duration = 1.0f/std::max(duration, 1e-7f);
|
||||
|
||||
for(int i=0; i<NUM_JOINTS; i++) {
|
||||
JointSpacePathSegment::start_pos[i] = start_pos[i];
|
||||
JointSpacePathSegment::end_pos[i] = end_pos[i];
|
||||
}
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
void JointSpacePathSegment::evaluate(
|
||||
float time,
|
||||
float joint_positions[NUM_JOINTS],
|
||||
float joint_velocity[NUM_JOINTS]) const
|
||||
{
|
||||
float t = time*inv_duration;
|
||||
float s = 1.0f-t;
|
||||
|
||||
for(int i=0; i<NUM_JOINTS; i++) {
|
||||
joint_positions[i] = start_pos[i]*s + end_pos[i]*t;
|
||||
joint_velocity[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
float JointSpacePathSegment::get_duration() {
|
||||
return duration;
|
||||
}
|
||||
|
||||
bool JointSpacePathSegment::is_initialized() {
|
||||
return initialized;
|
||||
}
|
||||
|
||||
//--- JointSpacePathSegmentGenerator ----------------------------------------------------
|
||||
|
||||
JointSpacePathSegmentGenerator::JointSpacePathSegmentGenerator(
|
||||
const CartesianPathSegment* path_segment,
|
||||
IKinemtaicModel* kinematic_model,
|
||||
float time_step)
|
||||
{
|
||||
JointSpacePathSegmentGenerator::path_segment = path_segment;
|
||||
JointSpacePathSegmentGenerator::kinematic_model = kinematic_model;
|
||||
current_time = 0.0f;
|
||||
delta_time = time_step;
|
||||
end_time = path_segment->get_duration();
|
||||
end_time_with_eps = end_time-0.2f*delta_time;
|
||||
|
||||
// check kinematic model
|
||||
if(kinematic_model->get_joint_count() != NUM_JOINTS) {
|
||||
LOG_ERROR("NUM_JOINTS (%i) differs from value required by Kinematic model (%i)",
|
||||
NUM_JOINTS, kinematic_model->get_joint_count());
|
||||
error_trap("Fatal Error");
|
||||
}
|
||||
|
||||
// evaluate inverse kinematic model to et start joint positions
|
||||
kinematic_model->inverse(path_segment->start_pose, current_joint_pos);
|
||||
}
|
||||
|
||||
bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_path_segment) {
|
||||
bool end_reached = false;
|
||||
|
||||
// increment evaluation position
|
||||
float initial_time = current_time;
|
||||
current_time += delta_time;
|
||||
|
||||
// check if end of path is reached, check against end_t which includes an epsilon
|
||||
// to prevent tiny segments at path end (snaps to t=1.0 within tolerance).
|
||||
if(current_time >= end_time_with_eps) {
|
||||
end_reached = true;
|
||||
current_time = end_time; // snap to 1.0
|
||||
}
|
||||
|
||||
// evaluate path to get new end position
|
||||
Pose6DF seg_end_pose;
|
||||
path_segment->evaluate(current_time, seg_end_pose);
|
||||
|
||||
// evaluate inverse kinematic model here
|
||||
float next_joint_pos[NUM_JOINTS];
|
||||
kinematic_model->inverse(seg_end_pose, next_joint_pos);
|
||||
|
||||
// create joint space path segment
|
||||
float duration = current_time-initial_time;
|
||||
js_path_segment = JointSpacePathSegment(current_joint_pos, next_joint_pos, duration);
|
||||
|
||||
// update current joint pos
|
||||
for(int i=0; i<NUM_JOINTS; i++)
|
||||
current_joint_pos[i] = next_joint_pos[i];
|
||||
|
||||
return end_reached;
|
||||
}
|
||||
137
firmware/MotionControllerRP/src/motion_control/path_segment.h
Normal file
137
firmware/MotionControllerRP/src/motion_control/path_segment.h
Normal file
|
|
@ -0,0 +1,137 @@
|
|||
#pragma once
|
||||
|
||||
//*** INCLUDE ***************************************************************************
|
||||
|
||||
#include "utilities/math3d.h"
|
||||
|
||||
//*** CONST *****************************************************************************
|
||||
|
||||
constexpr int NUM_JOINTS = 3;
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class IKinemtaicModel;
|
||||
|
||||
//--- JointInfo -------------------------------------------------------------------------
|
||||
|
||||
class JointInfo {
|
||||
public:
|
||||
float max_velocity;
|
||||
float max_acceleration;
|
||||
};
|
||||
|
||||
//--- MotionProfileConstAcc ------------------------------------------------------------
|
||||
|
||||
class MotionProfileConstAcc {
|
||||
public:
|
||||
MotionProfileConstAcc() = default;
|
||||
MotionProfileConstAcc(float distance,
|
||||
float v_start,
|
||||
float v_end,
|
||||
float max_velocity,
|
||||
float max_acceleration);
|
||||
MotionProfileConstAcc(float dwell_time);
|
||||
|
||||
// returns an interpolator value in range [0..1] that can be used to interpolate
|
||||
// start and end poses
|
||||
float evaluate(float time) const;
|
||||
|
||||
public:
|
||||
float t1 = 0.0f; // end time of accelleration phase
|
||||
float t2 = 0.0f; // end time of cruise phase
|
||||
float t3 = 0.0f; // end time of decellartion phase (total time)
|
||||
float acceleration = 1.0f;
|
||||
|
||||
float v_start;
|
||||
float v_end;
|
||||
|
||||
float v_peak;
|
||||
float d1; // distance after accelleration phase
|
||||
float d2; // distance after cruise phase
|
||||
};
|
||||
|
||||
//--- CartesianPathSegment --------------------------------------------------------------
|
||||
|
||||
// A Linear motion path segment in 6DOF Cartesian Space
|
||||
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);
|
||||
|
||||
void evaluate(float time, Pose6DF& pose) const;
|
||||
float get_duration() const;
|
||||
|
||||
void compute_motion_profile();
|
||||
|
||||
public:
|
||||
Pose6DF start_pose;
|
||||
Pose6DF end_pose;
|
||||
|
||||
LinearAngular start_velocity;
|
||||
LinearAngular target_velocity;
|
||||
LinearAngular end_velocity;
|
||||
LinearAngular max_acceleration;
|
||||
|
||||
LinearAngular travel_distance;
|
||||
MotionProfileConstAcc motion_profile;
|
||||
|
||||
float dwell_time; // stay at start position for given duration if dwell_time > 0
|
||||
};
|
||||
|
||||
//--- JointSpacePathSegment -------------------------------------------------------------
|
||||
|
||||
// A linear motion path segment in Joint Space
|
||||
class JointSpacePathSegment {
|
||||
public:
|
||||
JointSpacePathSegment();
|
||||
JointSpacePathSegment(const float start_pos[NUM_JOINTS],
|
||||
const float end_pos[NUM_JOINTS],
|
||||
const float duration);
|
||||
|
||||
void evaluate(float time,
|
||||
float joint_positions[NUM_JOINTS],
|
||||
float joint_velocity[NUM_JOINTS]) const;
|
||||
|
||||
float get_duration();
|
||||
|
||||
bool is_initialized();
|
||||
|
||||
public:
|
||||
bool initialized;
|
||||
float start_pos[NUM_JOINTS];
|
||||
float end_pos[NUM_JOINTS];
|
||||
float start_velocity[NUM_JOINTS];
|
||||
float end_velocity[NUM_JOINTS];
|
||||
|
||||
float duration;
|
||||
float inv_duration;
|
||||
};
|
||||
|
||||
//--- JointSpacePathSegmentGenerator ----------------------------------------------------
|
||||
|
||||
class JointSpacePathSegmentGenerator {
|
||||
public:
|
||||
JointSpacePathSegmentGenerator(
|
||||
const CartesianPathSegment* path_segment,
|
||||
IKinemtaicModel* kinematic_model,
|
||||
float time_step
|
||||
);
|
||||
|
||||
void reset();
|
||||
bool generate_next(JointSpacePathSegment& js_path_segment);
|
||||
|
||||
private:
|
||||
float delta_time; // time step size
|
||||
float current_time; // current t in range [0..1]
|
||||
float end_time; // end time
|
||||
float end_time_with_eps; // end time including a small negative epsilon
|
||||
float current_joint_pos[NUM_JOINTS]; // current joint positions
|
||||
|
||||
const CartesianPathSegment* path_segment = nullptr;
|
||||
IKinemtaicModel* kinematic_model;
|
||||
};
|
||||
|
||||
360
firmware/MotionControllerRP/src/robot.cpp
Normal file
360
firmware/MotionControllerRP/src/robot.cpp
Normal file
|
|
@ -0,0 +1,360 @@
|
|||
#include "robot.h"
|
||||
#include "hw_config.h"
|
||||
#include "utilities/logging.h"
|
||||
#include "kinemtaic_models/kinematic_model_delta3d.h"
|
||||
|
||||
//*** FUNCTION **************************************************************************
|
||||
|
||||
bool startswith(const std::string& str, const std::string& prefix) {
|
||||
return str.size() >= prefix.size() &&
|
||||
std::equal(prefix.begin(), prefix.end(), str.begin());
|
||||
}
|
||||
|
||||
//*** 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() {
|
||||
encoder->init(0x5, 0x4);
|
||||
servo_controller->init(0.5);
|
||||
}
|
||||
|
||||
void RobotJoint::home() {
|
||||
servo_controller->home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
|
||||
position = servo_controller->get_position();
|
||||
velocity = 0.0f;
|
||||
}
|
||||
|
||||
void RobotJoint::calibrate() {
|
||||
LookupTable lut;
|
||||
build_motor_to_enc_angle_lut(lut, *servo_controller, 1.0f*DEG_TO_RAD, 92.0f*DEG_TO_RAD, 256);
|
||||
// lut.print_to_log();
|
||||
LOG_DEBUG("Inverting lookup table...");
|
||||
bool ok = lut.invert(256);
|
||||
if(!ok) {
|
||||
servo_controller->get_motor_driver().disable();
|
||||
lut.print_to_log();
|
||||
while(true);
|
||||
}
|
||||
LOG_DEBUG(">finished");
|
||||
// lut.print_to_log();
|
||||
delay(200);
|
||||
|
||||
servo_controller->set_encoder_lut(lut);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//--- Robot -----------------------------------------------------------------------------
|
||||
|
||||
Robot::Robot(float path_segment_time_step) :
|
||||
path_planner(nullptr, path_segment_time_step),
|
||||
motion_controller(&path_planner),
|
||||
servo_loop_frequency_counter(10000),
|
||||
motion_controller_frequency_counter(1000)
|
||||
{
|
||||
kinematic_model = new KinematicModel_Delta3D();
|
||||
path_planner.set_kinematic_model(kinematic_model);
|
||||
|
||||
for(int i=0; i<3; i++)
|
||||
joints[i] = nullptr;
|
||||
|
||||
command_parser.set_command_processor(this);
|
||||
|
||||
max_acceleration = LinearAngular(500.0f, 50.0f);
|
||||
path_buffering_time_us = 100*1000;
|
||||
|
||||
state = ERobotState::IDLE;
|
||||
}
|
||||
|
||||
Robot::~Robot() {
|
||||
if(kinematic_model != nullptr)
|
||||
delete kinematic_model;
|
||||
|
||||
for(int i=0; i<3; i++) {
|
||||
if(joints[i] != nullptr)
|
||||
delete joints[i];
|
||||
joints[i] = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::init() {
|
||||
MT6835Encoder::setup_spi(spi0, PIN_ENCODER_SCK, PIN_ENCODER_MOSI, PIN_ENCODER_MISO, 8000000);
|
||||
|
||||
// axis 1
|
||||
{
|
||||
auto* encoder = new MT6835Encoder(spi0, PIN_ENCODER1_CS);
|
||||
auto* motor_driver = new TB6612MotorDriver(
|
||||
PIN_MOTOR_EN, PIN_M1_PWM_A_POS, PIN_M1_PWM_A_NEG, PIN_MOTOR_PWMAB,
|
||||
PIN_MOTOR_EN, PIN_M1_PWM_B_POS, PIN_M1_PWM_B_NEG, PIN_MOTOR_PWMAB
|
||||
);
|
||||
joints[0] = new RobotJoint(encoder, motor_driver, 400/4);
|
||||
}
|
||||
|
||||
// axis 2
|
||||
{
|
||||
auto* encoder = new MT6835Encoder(spi0, PIN_ENCODER2_CS);
|
||||
auto* motor_driver = new TB6612MotorDriver(
|
||||
PIN_MOTOR_EN, PIN_M2_PWM_A_POS, PIN_M2_PWM_A_NEG, PIN_MOTOR_PWMAB,
|
||||
PIN_MOTOR_EN, PIN_M2_PWM_B_POS, PIN_M2_PWM_B_NEG, PIN_MOTOR_PWMAB
|
||||
);
|
||||
joints[1] = new RobotJoint(encoder, motor_driver, 400/4);
|
||||
}
|
||||
|
||||
// axis 3
|
||||
{
|
||||
auto* encoder = new MT6835Encoder(spi0, PIN_ENCODER3_CS);
|
||||
auto* motor_driver = new TB6612MotorDriver(
|
||||
PIN_MOTOR_EN, PIN_M3_PWM_A_POS, PIN_M3_PWM_A_NEG, PIN_MOTOR_PWMAB,
|
||||
PIN_MOTOR_EN, PIN_M3_PWM_B_POS, PIN_M3_PWM_B_NEG, PIN_MOTOR_PWMAB
|
||||
);
|
||||
joints[2] = new RobotJoint(encoder, motor_driver, 400/4);
|
||||
}
|
||||
|
||||
// initialize axes
|
||||
for(int i=0; i<3; i++) {
|
||||
joints[i]->init();
|
||||
}
|
||||
|
||||
// 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;
|
||||
add_repeating_timer_us(-motion_controller_update_time_us,
|
||||
Robot::update_motion_controller_isr,
|
||||
(void*)this,
|
||||
&motion_controller_update_timer);
|
||||
}
|
||||
|
||||
void Robot::calibrate() {
|
||||
for(int i=0; i<3; i++) {
|
||||
joints[i]->calibrate();
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::home() {
|
||||
for(int i=0; i<3; i++) {
|
||||
joints[i]->home();
|
||||
joints[i]->calibrate();
|
||||
|
||||
// set start angle
|
||||
float start_angle = 20*Constants::DEG2RAD;
|
||||
joints[i]->servo_controller->move_to_open_loop(start_angle, 1.0f);
|
||||
if (spin_try_lock_unsafe(shared_data.lock)) {
|
||||
shared_data.joint_positions[i] = start_angle;
|
||||
spin_unlock_unsafe(shared_data.lock);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::update_command_parser() {
|
||||
// process serial input
|
||||
if (Serial.available()) {
|
||||
char c = Serial.read();
|
||||
command_parser.add_input_character(c);
|
||||
// Serial.write(c);
|
||||
}
|
||||
|
||||
// update command parse which will queue command to the path planner
|
||||
command_parser.update();
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the path planner, that chops up kartesian path segments into joint space
|
||||
* path segments using the inverse kinematic model. It then enqueues these joint space path
|
||||
* segments for the motion controller.
|
||||
*/
|
||||
void Robot::update_path_planner() {
|
||||
// check if buffering starts
|
||||
uint64_t time = time_us_64();
|
||||
if(state == ERobotState::IDLE && path_planner.input_queue_size() > 0) {
|
||||
state = ERobotState::BUFFERING_PAH;
|
||||
path_buffering_start_time = time;
|
||||
}
|
||||
|
||||
// check if execution starts
|
||||
uint64_t buffering_time = time-path_buffering_start_time;
|
||||
if(state == ERobotState::BUFFERING_PAH && buffering_time > path_buffering_time_us) {
|
||||
state = ERobotState::EXECUTING_PATH;
|
||||
path_buffering_start_time = time_us_64();
|
||||
}
|
||||
|
||||
// execute path
|
||||
if(state == ERobotState::EXECUTING_PATH) {
|
||||
// update planner and generate joint space path segments
|
||||
path_planner.process(true);
|
||||
|
||||
if(path_planner.all_finished())
|
||||
state = ERobotState::IDLE;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the motion controller with a timer interrupt in regular intervals.
|
||||
* 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];
|
||||
|
||||
// get robot pointer
|
||||
Robot* robot = (Robot*)timer->user_data;
|
||||
|
||||
// get time and delta time
|
||||
uint64_t time_us = time_us_64();
|
||||
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);
|
||||
|
||||
// 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)) {
|
||||
for (int i = 0; i < NUM_JOINTS; i++) {
|
||||
robot->shared_data.joint_positions[i] = joint_positions[i];
|
||||
robot->shared_data.joint_velocities[i] = joint_velocities[i];
|
||||
}
|
||||
spin_unlock_unsafe(robot->shared_data.lock);
|
||||
}
|
||||
|
||||
// update frequency counter
|
||||
robot->motion_controller_frequency_counter.update(dt);
|
||||
|
||||
return true; // keep repeating
|
||||
}
|
||||
|
||||
/**
|
||||
* update servo loops, this is called from a second cpu core
|
||||
*/
|
||||
void Robot::update_servo_controllers(float dt) {
|
||||
float one_over_dt = 1.0f/dt;
|
||||
|
||||
// update axis target position and velocity from shared data
|
||||
spin_lock_unsafe_blocking(shared_data.lock);
|
||||
for(int i=0; i<3; i++)
|
||||
joints[i]->update_target(shared_data.joint_positions[i], shared_data.joint_velocities[i]);
|
||||
spin_unlock_unsafe(shared_data.lock);
|
||||
|
||||
// update servo loop for each axis
|
||||
for(int i=0; i<3; i++) {
|
||||
joints[i]->update(dt, one_over_dt);
|
||||
}
|
||||
|
||||
// update frequency counter
|
||||
servo_loop_frequency_counter.update(dt);
|
||||
}
|
||||
|
||||
bool Robot::can_process_command(const GCodeCommand& cmd) {
|
||||
if(cmd.get_command() == "G0" ||
|
||||
cmd.get_command() == "G4")
|
||||
{
|
||||
return path_planner.input_queue_full() == false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Robot::send_reply(const char* str) {
|
||||
Serial.write(str);
|
||||
}
|
||||
|
||||
void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
if(cmd.get_command() == "G0") process_motion_command(cmd, reply);
|
||||
else if(cmd.get_command() == "G4") process_dwell_command(cmd, reply);
|
||||
else if(startswith(cmd.get_command(), "M")) process_machine_command(cmd, reply);
|
||||
else reply="error: unknown command\n";
|
||||
}
|
||||
|
||||
void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
Pose6DF end_pose;
|
||||
|
||||
// read feed rate
|
||||
float feed_linear = cmd.get_value('F', 10.0f);
|
||||
float feed_angular = cmd.get_value('R', 1.0f);
|
||||
|
||||
// read translation
|
||||
end_pose.translation.x = cmd.get_value('X', current_pose.translation.x);
|
||||
end_pose.translation.y = cmd.get_value('Y', current_pose.translation.y);
|
||||
end_pose.translation.z = cmd.get_value('Z', current_pose.translation.z);
|
||||
|
||||
// read rotation (all elements must be present)
|
||||
if(cmd.has_word('A') && cmd.has_word('B') && cmd.has_word('C')) {
|
||||
Vec3F rot_vec(cmd.get_value('A'), cmd.get_value('B'), cmd.get_value('C'));
|
||||
end_pose.rotation = QuaternionF::from_rot_vec(rot_vec);
|
||||
} else {
|
||||
end_pose.rotation = current_pose.rotation;
|
||||
}
|
||||
|
||||
// create path segment
|
||||
CartesianPathSegment path_segment(current_pose, end_pose,
|
||||
LinearAngular(feed_linear, feed_angular),
|
||||
max_acceleration);
|
||||
|
||||
path_planner.add_cartesian_path_segment(path_segment);
|
||||
current_pose = end_pose;
|
||||
|
||||
reply = "ok\n";
|
||||
}
|
||||
|
||||
void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
if(cmd.get_command() == "M50") {
|
||||
reply = "Current Position: ";
|
||||
reply += std::string(" X") + std::to_string(current_pose.translation.x);
|
||||
reply += std::string(" Y") + std::to_string(current_pose.translation.y);
|
||||
reply += std::string(" Z") + std::to_string(current_pose.translation.z);
|
||||
reply += "\n";
|
||||
}
|
||||
if(cmd.get_command() == "M51") {
|
||||
uint32_t servo_loop_freq = servo_loop_frequency_counter.get();
|
||||
uint32_t mcontroler_freq = motion_controller_frequency_counter.get();
|
||||
reply += std::string("Servo Loop: ") + std::to_string(servo_loop_freq/1000) + "kHz\n";
|
||||
reply += std::string("Motion Controler: ") + std::to_string(mcontroler_freq/1000) + "kHz\n";
|
||||
}
|
||||
if(cmd.get_command() == "M204") {
|
||||
|
||||
if(cmd.has_word('L')) max_acceleration.linear = cmd.get_value('L');
|
||||
if(cmd.has_word('A')) max_acceleration.angular = cmd.get_value('A');
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
|
||||
// get dwell time
|
||||
float dwell_time = 1.0f;
|
||||
if(cmd.has_word('S')) dwell_time = cmd.get_value('S'); // time given in seconds
|
||||
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);
|
||||
|
||||
reply = "ok\n";
|
||||
}
|
||||
|
||||
|
||||
113
firmware/MotionControllerRP/src/robot.h
Normal file
113
firmware/MotionControllerRP/src/robot.h
Normal file
|
|
@ -0,0 +1,113 @@
|
|||
#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 "servo_control/encoder_lut.h"
|
||||
#include "utilities/math_constants.h"
|
||||
|
||||
#include "motion_control/path_planner.h"
|
||||
#include "motion_control/motion_controller.h"
|
||||
#include "command_parser/command_parser.h"
|
||||
|
||||
//*** CALSS *****************************************************************************
|
||||
|
||||
class Robot;
|
||||
|
||||
//--- SharedData ------------------------------------------------------------------------
|
||||
|
||||
enum class ERobotState {
|
||||
IDLE = 0,
|
||||
BUFFERING_PAH = 1,
|
||||
EXECUTING_PATH = 2,
|
||||
ERROR = 3
|
||||
};
|
||||
|
||||
//--- SharedData ------------------------------------------------------------------------
|
||||
|
||||
struct SharedData {
|
||||
SharedData(int hw_spinlock_id=0){
|
||||
lock = spin_lock_instance(hw_spinlock_id);
|
||||
};
|
||||
|
||||
volatile float joint_positions[NUM_JOINTS];
|
||||
volatile float joint_velocities[NUM_JOINTS];
|
||||
spin_lock_t* lock = nullptr;
|
||||
};
|
||||
|
||||
//--- RobotJoint ------------------------------------------------------------------------
|
||||
|
||||
class RobotJoint {
|
||||
public:
|
||||
RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
|
||||
~RobotJoint();
|
||||
|
||||
void init();
|
||||
void home();
|
||||
void calibrate();
|
||||
void update(float dt, float one_over_dt);
|
||||
|
||||
void update_target(float p, float v);
|
||||
|
||||
public:
|
||||
float position;
|
||||
float velocity;
|
||||
|
||||
MT6835Encoder* encoder;
|
||||
TB6612MotorDriver* motor_driver;
|
||||
ServoController* servo_controller;
|
||||
};
|
||||
|
||||
//--- Robot -----------------------------------------------------------------------------
|
||||
|
||||
class Robot : public ICommandProcessor {
|
||||
public:
|
||||
Robot(float path_segment_time_step);
|
||||
~Robot();
|
||||
|
||||
void init();
|
||||
void calibrate();
|
||||
void home();
|
||||
|
||||
void update_command_parser(); // called from main loop
|
||||
void update_path_planner(); // called from main loop
|
||||
void update_servo_controllers(float dt); // called from seperate cpu-core
|
||||
|
||||
public:
|
||||
void send_reply(const char* str) override;
|
||||
bool can_process_command(const GCodeCommand& cmd) override;
|
||||
void process_command(const GCodeCommand& cmd, std::string& reply) override;
|
||||
|
||||
void process_motion_command(const GCodeCommand& cmd, std::string& reply);
|
||||
void process_dwell_command(const GCodeCommand& cmd, std::string& reply);
|
||||
void process_machine_command(const GCodeCommand& cmd, std::string& reply);
|
||||
|
||||
protected:
|
||||
static bool update_motion_controller_isr(repeating_timer_t* timer); // called from update timer
|
||||
|
||||
private:
|
||||
ERobotState state;
|
||||
uint32_t path_buffering_time_us;
|
||||
uint64_t path_buffering_start_time;
|
||||
|
||||
RobotJoint* volatile joints[NUM_JOINTS];
|
||||
|
||||
IKinemtaicModel* kinematic_model;
|
||||
PathPlanner path_planner;
|
||||
MotionController motion_controller;
|
||||
CommandParser command_parser;
|
||||
|
||||
LinearAngular max_acceleration;
|
||||
Pose6DF current_pose;
|
||||
|
||||
SharedData shared_data;
|
||||
|
||||
struct repeating_timer motion_controller_update_timer;
|
||||
uint64_t last_mc_update_time;
|
||||
|
||||
FrequencyCounter servo_loop_frequency_counter;
|
||||
FrequencyCounter motion_controller_frequency_counter;
|
||||
};
|
||||
176
firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp
Normal file
176
firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp
Normal file
|
|
@ -0,0 +1,176 @@
|
|||
#include "encoder_lut.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "utilities/logging.h"
|
||||
#include "utilities/math_constants.h"
|
||||
|
||||
void LookupTable::init(int32_t size, float input_min, float input_max) {
|
||||
lookup_table.clear();
|
||||
lookup_table.resize(size, 0.0f);
|
||||
LookupTable::input_min = input_min;
|
||||
LookupTable::input_max = input_max;
|
||||
LookupTable::one_over_input_range = 1.0f/(input_max-input_min);
|
||||
}
|
||||
|
||||
void LookupTable::clear() {
|
||||
lookup_table.clear();
|
||||
}
|
||||
|
||||
// returns the size of the lookup table
|
||||
uint32_t LookupTable::size() {
|
||||
return (uint32_t)lookup_table.size();
|
||||
}
|
||||
|
||||
void LookupTable::set_entry(int32_t idx, float v) {
|
||||
lookup_table[idx] = v;
|
||||
}
|
||||
|
||||
// set an entry of the lookup table
|
||||
float LookupTable::get_entry(int32_t idx) {
|
||||
return lookup_table[idx];
|
||||
}
|
||||
|
||||
float LookupTable::evaluate(float x) const {
|
||||
if (lookup_table.empty() || lookup_table.size() < 2)
|
||||
return 0.0f;
|
||||
|
||||
int32_t table_size = lookup_table.size();
|
||||
float t = (x - input_min) * one_over_input_range;
|
||||
float pos = t * (table_size - 1);
|
||||
float frac;
|
||||
size_t index;
|
||||
|
||||
if (t < 0.0f) {
|
||||
return lookup_table.front();
|
||||
// Extrapolate to the left using first two points
|
||||
index = 0;
|
||||
frac = pos; // pos is negative
|
||||
} else if (t >= 1.0f) {
|
||||
return lookup_table.back();
|
||||
// Extrapolate to the right using last two points
|
||||
index = table_size - 2;
|
||||
frac = pos - (table_size - 2);
|
||||
} else {
|
||||
// Interpolate normally
|
||||
index = static_cast<size_t>(std::floor(pos));
|
||||
frac = pos - index;
|
||||
}
|
||||
|
||||
float a = lookup_table[index];
|
||||
float b = lookup_table[index + 1];
|
||||
|
||||
return a + frac * (b - a); // Linear interpolation or extrapolation
|
||||
}
|
||||
|
||||
|
||||
bool LookupTable::is_monotonic() const {
|
||||
if (lookup_table.size() < 2)
|
||||
return true;
|
||||
|
||||
bool increasing = true, decreasing = true;
|
||||
for (size_t i = 1; i < lookup_table.size(); ++i) {
|
||||
float b = lookup_table[i - 1];
|
||||
float a = lookup_table[i];
|
||||
|
||||
if (a < b) increasing = false;
|
||||
if (a > b) decreasing = false;
|
||||
}
|
||||
|
||||
return increasing || decreasing;
|
||||
}
|
||||
|
||||
bool almost_equal(float a, float b, float rel_tol = 1e-6f, float abs_tol = 1e-6f) {
|
||||
return std::fabs(a - b) <= std::max(rel_tol * std::max(std::fabs(a), std::fabs(b)), abs_tol);
|
||||
}
|
||||
|
||||
float LookupTable::evaluate_inverse(float y) const {
|
||||
int size = static_cast<int>(lookup_table.size());
|
||||
if (size < 2) return input_min;
|
||||
|
||||
int low = 0;
|
||||
int high = size - 1;
|
||||
bool increasing = lookup_table.front() < lookup_table.back();
|
||||
|
||||
// Clamp y outside the range
|
||||
// Clamp y outside the range
|
||||
if ((increasing && y <= lookup_table.front()) ||
|
||||
(!increasing && y >= lookup_table.front()))
|
||||
return input_min;
|
||||
if ((increasing && y >= lookup_table.back()) ||
|
||||
(!increasing && y <= lookup_table.back()))
|
||||
return input_max;
|
||||
|
||||
// Binary search to find the interval
|
||||
while (high - low > 1) {
|
||||
int mid = (low + high) / 2;
|
||||
float val = lookup_table[mid];
|
||||
|
||||
if ((increasing && val < y) || (!increasing && val > y))
|
||||
low = mid;
|
||||
else
|
||||
high = mid;
|
||||
}
|
||||
|
||||
// Interpolate between low and high
|
||||
float y0 = lookup_table[low];
|
||||
float y1 = lookup_table[high];
|
||||
|
||||
if (std::fabs(y1 - y0) < std::numeric_limits<float>::epsilon()) {
|
||||
// Avoid division by zero if both entries are equal
|
||||
float t = float(low) / (size - 1);
|
||||
return input_min + t * (input_max - input_min);
|
||||
}
|
||||
|
||||
float t = (y - y0) / (y1 - y0);
|
||||
float pos = (float(low) + t) / (size - 1);
|
||||
|
||||
return input_min + pos * (input_max - input_min);
|
||||
}
|
||||
|
||||
// inverts the lookup table so it represents the funcion x = fi(y) given y = f(x)
|
||||
bool LookupTable::invert(int new_size) {
|
||||
if (lookup_table.empty() || new_size <= 0) {
|
||||
LOG_ERROR("invert_lut(): lut size is zero");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!is_monotonic()) {
|
||||
LOG_ERROR("invert_lut(): lut is not monotonic");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Find the output (y) range of the current LUT
|
||||
float output_min = lookup_table.front();
|
||||
float output_max = lookup_table.back();
|
||||
if (output_max < output_min) {
|
||||
std::swap(output_min, output_max);
|
||||
}
|
||||
|
||||
// Prepare new LUT data
|
||||
std::vector<float> new_lut(new_size);
|
||||
float delta_y = (output_max - output_min) / (new_size - 1);
|
||||
|
||||
for (int i = 0; i < new_size; ++i) {
|
||||
float y = output_min + i * delta_y;
|
||||
new_lut[i] = evaluate_inverse(y); // find x for given y
|
||||
}
|
||||
|
||||
// Replace old LUT with the inverted LUT
|
||||
lookup_table = std::move(new_lut);
|
||||
input_min = output_min;
|
||||
input_max = output_max;
|
||||
one_over_input_range = 1.0f/(input_max-input_min);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void LookupTable::print_to_log() const {
|
||||
int size = lookup_table.size();
|
||||
if (size == 0) return;
|
||||
|
||||
float step = (input_max - input_min) / (size - 1);
|
||||
for (int i = 0; i < size; ++i) {
|
||||
float x = input_min + i * step;
|
||||
float y = lookup_table[i];
|
||||
LOG_INFO("%.6f;%.6f", x, y);
|
||||
}
|
||||
}
|
||||
50
firmware/MotionControllerRP/src/servo_control/encoder_lut.h
Normal file
50
firmware/MotionControllerRP/src/servo_control/encoder_lut.h
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
class LookupTable {
|
||||
public:
|
||||
LookupTable() {}
|
||||
|
||||
// initializes the lookup table to a given size and input range
|
||||
void init(int32_t size, float input_min, float input_max);
|
||||
|
||||
// clear the lookup table, use init to use it again
|
||||
void clear();
|
||||
|
||||
// returns the size of the lookup table
|
||||
uint32_t size();
|
||||
|
||||
// set an entry of the lookup table
|
||||
void set_entry(int32_t idx, float v);
|
||||
|
||||
// set an entry of the lookup table
|
||||
float get_entry(int32_t idx);
|
||||
|
||||
// evaluate the lookup table at a given position with linear interpolation
|
||||
float evaluate(float x) const;
|
||||
|
||||
// evaluate the inverse of the lookup table function (very slow), the LUT must be monotonic
|
||||
float evaluate_inverse(float y) const;
|
||||
|
||||
// inverts the lookup table so it represents the funcion x = fi(y) given y = f(x)
|
||||
bool invert(int new_size);
|
||||
|
||||
// check if the lookup table is monotonic
|
||||
bool is_monotonic() const;
|
||||
|
||||
// prints the lookup table using the logger
|
||||
void print_to_log() const;
|
||||
|
||||
private:
|
||||
float input_min = 0.0f;
|
||||
float input_max = 0.0f;
|
||||
float one_over_input_range = 1.0f;
|
||||
std::vector<float> lookup_table;
|
||||
};
|
||||
|
||||
//*** FUNCTION ***********************************************************************************/
|
||||
|
||||
70
firmware/MotionControllerRP/src/servo_control/pid.cpp
Normal file
70
firmware/MotionControllerRP/src/servo_control/pid.cpp
Normal file
|
|
@ -0,0 +1,70 @@
|
|||
#include "pid.h"
|
||||
#include <algorithm>
|
||||
|
||||
PIDController::PIDController()
|
||||
: kP(0.0f), kI(0.0f), kD(0.0f), kI_half(0.0f)
|
||||
, output_limit(0.0f), windup_limit(0.0f)
|
||||
, error_prev(0.0f), integral_prev(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
void PIDController::set_parameter(float kP, float kI, float kD, float output_limit, float windup_limit) {
|
||||
PIDController::kP = kP;
|
||||
PIDController::kI = kI;
|
||||
PIDController::kD = kD;
|
||||
PIDController::output_limit = output_limit;
|
||||
PIDController::windup_limit = windup_limit;
|
||||
|
||||
PIDController::kI_half = kI*0.5f;
|
||||
}
|
||||
|
||||
// PID controller function
|
||||
float PIDController::compute(float error, float dt, float one_over_dt) {
|
||||
// Proportional component
|
||||
float proportional = kP * error;
|
||||
float output = proportional;
|
||||
|
||||
// Integral component
|
||||
if(kI != 0.0f) {
|
||||
// Tustin transform of the integral part
|
||||
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
|
||||
float integral = integral_prev + kI_half*dt*(error + error_prev);
|
||||
integral = std::clamp(integral, -windup_limit, windup_limit);
|
||||
output += integral;
|
||||
integral_prev = integral;
|
||||
}
|
||||
|
||||
// Derivative component
|
||||
if(kD != 0.0f) {
|
||||
// u_dk = D(ek - ek_1)/Ts
|
||||
float derivative = kD*(error - error_prev)*one_over_dt;
|
||||
output += derivative;
|
||||
}
|
||||
|
||||
// clamp output and store error
|
||||
output = std::clamp(output, -output_limit, output_limit);
|
||||
error_prev = error;
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
void PIDController::reset(){
|
||||
integral_prev = 0.0f;
|
||||
error_prev = 0.0f;
|
||||
}
|
||||
|
||||
//--- LowpassFilter -----------------------------------------------------------
|
||||
|
||||
LowpassFilter::LowpassFilter(): value_prev(0.0f), time_constant(1.0f) {
|
||||
}
|
||||
|
||||
void LowpassFilter::set_time_constant(float time_constant) {
|
||||
LowpassFilter::time_constant = time_constant;
|
||||
}
|
||||
|
||||
float LowpassFilter::update(float value, float dt) {
|
||||
float alpha = time_constant/(time_constant + dt);
|
||||
float v = value_prev*alpha + (1.0f - alpha)*value;
|
||||
value_prev = v;
|
||||
return v;
|
||||
}
|
||||
40
firmware/MotionControllerRP/src/servo_control/pid.h
Normal file
40
firmware/MotionControllerRP/src/servo_control/pid.h
Normal file
|
|
@ -0,0 +1,40 @@
|
|||
#pragma once
|
||||
|
||||
//--- LowpassFilter -----------------------------------------------------------
|
||||
|
||||
class LowpassFilter {
|
||||
public:
|
||||
LowpassFilter();
|
||||
|
||||
void set_time_constant(float time_constant);
|
||||
float update(float value, float dt);
|
||||
|
||||
private:
|
||||
float value_prev;
|
||||
float time_constant;
|
||||
};
|
||||
|
||||
//--- PIDController -----------------------------------------------------------
|
||||
|
||||
class PIDController {
|
||||
public:
|
||||
PIDController();
|
||||
~PIDController() = default;
|
||||
|
||||
void set_parameter(float kP, float kI, float kD, float output_limit, float windup_limit);
|
||||
float compute(float error, float dt, float one_over_dt);
|
||||
void reset();
|
||||
|
||||
protected:
|
||||
float output_limit; // Maximum output value
|
||||
float windup_limit; // Maximum output value
|
||||
|
||||
float kP; // Proportional gain
|
||||
float kI; // Integral gain
|
||||
float kD; // Derivative gain
|
||||
|
||||
float error_prev; // last tracking error value
|
||||
float integral_prev; // last integral component value
|
||||
|
||||
float kI_half; // to avoid multiply
|
||||
};
|
||||
|
|
@ -0,0 +1,283 @@
|
|||
#include "hardware/timer.h"
|
||||
#include "Arduino.h"
|
||||
|
||||
#include "servo_controller.h"
|
||||
#include "utilities/logging.h"
|
||||
#include "utilities/math_constants.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
ServoController::ServoController(
|
||||
MOTOR_DRIVER_TYPE& motor_driver,
|
||||
ENCODER_TYPE& encoder,
|
||||
int32_t motor_pole_pair_count) :
|
||||
motor_driver(motor_driver),
|
||||
encoder(encoder),
|
||||
motorpos_to_field_angle(motor_pole_pair_count)
|
||||
{
|
||||
motor_pos = 0.0f;
|
||||
pos_error = 0.0f;
|
||||
}
|
||||
|
||||
void ServoController::init(float max_motor_amplitude) {
|
||||
ServoController::motor_current_amplitude = max_motor_amplitude;
|
||||
|
||||
// setup motor driver
|
||||
motor_driver.begin();
|
||||
motor_driver.set_amplitude(0.0f, true);
|
||||
motor_driver.enable();
|
||||
motor_driver.set_field_angle(0.0f);
|
||||
|
||||
// soft start
|
||||
for(int i=0; i<100; i++) {
|
||||
motor_driver.set_amplitude(motor_current_amplitude*float(i)/(100-1), true);
|
||||
sleep_ms(1);
|
||||
}
|
||||
|
||||
velocity_lowpass.set_time_constant(0.0025f);
|
||||
pos_controller.set_parameter(150.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
|
||||
velocity_controller.set_parameter(0.2f, 100.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
|
||||
}
|
||||
|
||||
void ServoController::set_encoder_lut(LookupTable& enc_to_pos_lut) {
|
||||
ServoController::enc_to_pos_lut = enc_to_pos_lut;
|
||||
}
|
||||
|
||||
void ServoController::update(float target_motor_pos, float dt, float one_over_dt) {
|
||||
// read encoder
|
||||
int32_t encoder_angle_raw = encoder.read_abs_angle_raw();
|
||||
|
||||
// convert encoder angle to motor pos using LUT and compute field angle
|
||||
motor_pos = encoder_angle_to_motor_pos(encoder_angle_raw);
|
||||
float field_angle = motor_pos_to_field_angle(motor_pos);
|
||||
|
||||
// position controll loop
|
||||
pos_error = target_motor_pos-motor_pos;
|
||||
float velocity_target = pos_controller.compute(pos_error, dt, one_over_dt);
|
||||
|
||||
// velocity controll loop
|
||||
float velocity_unfiltered = (motor_pos - motor_pos_prev)*one_over_dt;
|
||||
velocity = velocity_lowpass.update(velocity_unfiltered, dt);
|
||||
float torque_target = velocity_controller.compute(velocity_target-velocity, dt, one_over_dt);
|
||||
|
||||
// torque controll loop
|
||||
output = torque_target;
|
||||
|
||||
// set new field direction
|
||||
// motor_driver.set_amplitude(std::clamp(abs(output*10.0f), 0.1f, 0.5f), false);
|
||||
motor_driver.set_field_angle(field_angle + output);
|
||||
|
||||
// store values for next update
|
||||
motor_pos_prev = motor_pos;
|
||||
}
|
||||
|
||||
bool ServoController::at_position(float motor_pos_eps) {
|
||||
return fabs(pos_error) < motor_pos_eps;
|
||||
}
|
||||
|
||||
float ServoController::read_position() {
|
||||
return encoder_angle_to_motor_pos(encoder.read_abs_angle_raw());
|
||||
}
|
||||
|
||||
float ServoController::get_position() {
|
||||
return motor_pos;
|
||||
}
|
||||
|
||||
float ServoController::get_position_error() {
|
||||
return pos_error;
|
||||
}
|
||||
|
||||
bool ServoController::move_to(float target_motor_pos, float at_pos_eps, float settle_time_s, float timeout_s) {
|
||||
uint64_t start_time_us = time_us_64();
|
||||
uint64_t time_us = start_time_us;
|
||||
uint64_t pos_reached_time_us = 0;
|
||||
uint64_t last_time = time_us;
|
||||
uint32_t settle_time_us = settle_time_s*1e6f;
|
||||
uint32_t timeout_us = timeout_s*1e6f;
|
||||
|
||||
do {
|
||||
// get time and detla time
|
||||
time_us = time_us_64();
|
||||
float dt = float(time_us - last_time)*1e-6f;
|
||||
last_time = time_us;
|
||||
|
||||
float pos_error;
|
||||
update(target_motor_pos, dt, 1.0f/dt);
|
||||
|
||||
// check if traget position reached
|
||||
if(pos_reached_time_us == 0) {
|
||||
if(at_position(at_pos_eps))
|
||||
pos_reached_time_us = time_us;
|
||||
} else {
|
||||
if(time_us-pos_reached_time_us > settle_time_us)
|
||||
return true;
|
||||
}
|
||||
|
||||
} while(time_us-start_time_us < timeout_us);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void ServoController::move_to_open_loop(float target_motor_pos, float motor_angular_velocity) {
|
||||
// Determine direction of movement at the start
|
||||
const bool moving_forward = target_motor_pos > motor_pos;
|
||||
|
||||
uint64_t last_time = time_us_64();
|
||||
while ((moving_forward && motor_pos < target_motor_pos) ||
|
||||
(!moving_forward && motor_pos > target_motor_pos))
|
||||
{
|
||||
uint64_t time_us = time_us_64();
|
||||
float dt = float(time_us - last_time) * 1e-6f;
|
||||
last_time = time_us;
|
||||
|
||||
// update encoder regularly
|
||||
encoder.read_abs_angle_raw();
|
||||
|
||||
// update motor position
|
||||
motor_pos += moving_forward ? motor_angular_velocity * dt : -motor_angular_velocity * dt;
|
||||
|
||||
// set field ange to new position
|
||||
float clamped_motor_pos = moving_forward ? std::min(motor_pos, target_motor_pos) :
|
||||
std::max(motor_pos, target_motor_pos);
|
||||
motor_driver.set_field_angle(motor_pos_to_field_angle(clamped_motor_pos));
|
||||
sleep_us(100);
|
||||
}
|
||||
|
||||
motor_pos = target_motor_pos;
|
||||
}
|
||||
|
||||
void ServoController::home(float motor_velocity, float search_range, float current) {
|
||||
bool search_failed = false;
|
||||
float pos_offset = 0.0f;
|
||||
motor_driver.set_amplitude(current, true);
|
||||
float eval_pos_delta = (Constants::TWO_PI_F*0.1)/motorpos_to_field_angle;
|
||||
|
||||
// determine expected encoder angle delta for motion of eval_pos_delta
|
||||
motor_driver.set_field_angle(motor_pos_to_field_angle(motor_pos+eval_pos_delta));
|
||||
sleep_ms(200);
|
||||
float angle1 = encoder.read_abs_angle();
|
||||
|
||||
motor_driver.set_field_angle(motor_pos_to_field_angle(motor_pos));
|
||||
sleep_ms(200);
|
||||
float angle2 = encoder.read_abs_angle();
|
||||
float expected_encoder_delta = (angle2-angle1);
|
||||
|
||||
// start homing search
|
||||
uint64_t last_time = time_us_64();
|
||||
float encoder_angle_prev = encoder.read_abs_angle();
|
||||
float last_eval_offset = 0.0f;
|
||||
|
||||
while(true) {
|
||||
// compute time delta
|
||||
uint64_t time_us = time_us_64();
|
||||
float dt = float(time_us - last_time) * 1e-6f;
|
||||
last_time = time_us;
|
||||
|
||||
// move motor and read encoder
|
||||
pos_offset += motor_velocity * dt;
|
||||
motor_driver.set_field_angle(motor_pos_to_field_angle(motor_pos+pos_offset));
|
||||
float encoder_angle = encoder.read_abs_angle();
|
||||
|
||||
// check ratio of measured encoder delta to expected delta to determine motor stop
|
||||
if(fabs(last_eval_offset-pos_offset) > eval_pos_delta) {
|
||||
float encoder_delta = (encoder_angle - encoder_angle_prev);
|
||||
float encoder_velocity_ratio = encoder_delta/expected_encoder_delta;
|
||||
// Serial.printf(">encoder_velocity_ratio: %f\n", encoder_velocity_ratio);
|
||||
// Serial.printf(">encoder_velocity: %f\n", encoder_velocity);
|
||||
if(encoder_velocity_ratio < 0.05f)
|
||||
break;
|
||||
encoder_angle_prev = encoder_angle;
|
||||
last_eval_offset = pos_offset;
|
||||
}
|
||||
|
||||
// check if search range exeeded
|
||||
if(fabs(pos_offset) > search_range) {
|
||||
search_failed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// reset positions
|
||||
motor_pos = 0;
|
||||
motor_driver.set_field_angle(0);
|
||||
sleep_ms(200);
|
||||
encoder.reset_abs_angle();
|
||||
|
||||
// set normal motor current
|
||||
motor_driver.set_amplitude(motor_current_amplitude, true);
|
||||
}
|
||||
|
||||
ServoController::ENCODER_TYPE& ServoController::get_encoder() {
|
||||
return encoder;
|
||||
}
|
||||
|
||||
ServoController::MOTOR_DRIVER_TYPE& ServoController::get_motor_driver() {
|
||||
return motor_driver;
|
||||
}
|
||||
|
||||
float ServoController::encoder_angle_to_motor_pos(int32_t encoder_angle_raw) {
|
||||
// TODO: use lut here
|
||||
if(enc_to_pos_lut.size() == 0) {
|
||||
int32_t encoder_cpr = encoder.get_rawcounts_per_rev();
|
||||
return encoder_angle_raw*Constants::TWO_PI_F/encoder_cpr/(7.5f*4);
|
||||
} else {
|
||||
return enc_to_pos_lut.evaluate(encoder_angle_raw);
|
||||
}
|
||||
}
|
||||
|
||||
float ServoController::motor_pos_to_field_angle(float motor_pos) {
|
||||
return motor_pos*motorpos_to_field_angle;
|
||||
}
|
||||
|
||||
float ServoController::motor_velocity_to_field_velocity(float v) {
|
||||
return v*motorpos_to_field_angle;
|
||||
}
|
||||
|
||||
//*** FUNCTION ***********************************************************************************/
|
||||
|
||||
bool build_motor_to_enc_angle_lut(
|
||||
LookupTable& lut,
|
||||
ServoController& servo_controller,
|
||||
float min_motor_angle,
|
||||
float max_motor_angle,
|
||||
size_t size)
|
||||
{
|
||||
LOG_INFO("Measuring motor to encoder angle lookup table...");
|
||||
float speed = 1.0f;
|
||||
float input_min = min_motor_angle;
|
||||
float input_max = max_motor_angle;
|
||||
|
||||
lut.init(size, input_min, input_max);
|
||||
// float initial_pos = servo_controller.read_position();
|
||||
// move to starting position
|
||||
servo_controller.move_to_open_loop(min_motor_angle, 2.0f);
|
||||
servo_controller.get_encoder().reset_abs_angle(0); // Reset encoder to 0 at min_motor_angle
|
||||
|
||||
float step = float(input_max - input_min) / (size - 1);
|
||||
|
||||
// Measure in increasing direction
|
||||
for (size_t i = 0; i < size; ++i) {
|
||||
float target_motor_angle = input_min + step * i;
|
||||
servo_controller.move_to_open_loop(target_motor_angle, speed);
|
||||
// sleep_ms(0);
|
||||
float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
|
||||
lut.set_entry(i, encoder_angle_raw);
|
||||
}
|
||||
|
||||
// Measure in decreasing direction (average with increasing direction)
|
||||
for (size_t i = 0; i < size; ++i) {
|
||||
float target_motor_angle = input_max - step * i; // Start from max and go down
|
||||
servo_controller.move_to_open_loop(target_motor_angle, speed);
|
||||
// sleep_ms(0);
|
||||
float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
|
||||
// Average with the previously recorded value
|
||||
int idx = size-1-i;
|
||||
lut.set_entry(idx, (lut.get_entry(idx) + encoder_angle_raw) / 2.0f);
|
||||
}
|
||||
|
||||
// move to starting position
|
||||
servo_controller.move_to_open_loop(min_motor_angle, 2.0f);
|
||||
|
||||
LOG_INFO(">finished");
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#pragma once
|
||||
|
||||
#include "hardware/MT6835_encoder.h"
|
||||
#include "hardware/TB6612_motor_driver.h"
|
||||
#include "encoder_lut.h"
|
||||
#include "pid.h"
|
||||
|
||||
class ServoController {
|
||||
public:
|
||||
// use defines instead of virtual functions for speed
|
||||
// TODO: check if this makes any difference and change accordingly
|
||||
typedef TB6612MotorDriver MOTOR_DRIVER_TYPE;
|
||||
typedef MT6835Encoder ENCODER_TYPE;
|
||||
|
||||
public:
|
||||
ServoController(MOTOR_DRIVER_TYPE& motor_driver, ENCODER_TYPE& encoder, int32_t motor_pole_pairs);
|
||||
|
||||
void init(float max_motor_amplitude);
|
||||
|
||||
void set_encoder_lut(LookupTable& enc_to_pos_lut);
|
||||
|
||||
void update(float target_motor_pos,
|
||||
float dt,
|
||||
float one_over_dt);
|
||||
|
||||
bool at_position(float motor_pos_eps);
|
||||
|
||||
float read_position();
|
||||
|
||||
float get_position();
|
||||
|
||||
float get_position_error();
|
||||
|
||||
bool move_to(float target_motor_angle,
|
||||
float at_pos_motor_angle_eps,
|
||||
float settle_time_ms,
|
||||
float timeout_us);
|
||||
|
||||
void move_to_open_loop(float target_motor_angle,
|
||||
float angular_velocity);
|
||||
|
||||
void home(float motor_velocity, float search_range, float current=0.2f);
|
||||
|
||||
ENCODER_TYPE& get_encoder();
|
||||
MOTOR_DRIVER_TYPE& get_motor_driver();
|
||||
float output;
|
||||
|
||||
private:
|
||||
float encoder_angle_to_motor_pos(int32_t encoder_angle_raw);
|
||||
float motor_pos_to_field_angle(float motor_pos);
|
||||
float motor_velocity_to_field_velocity(float v);
|
||||
|
||||
private:
|
||||
ENCODER_TYPE& encoder;
|
||||
MOTOR_DRIVER_TYPE& motor_driver;
|
||||
LookupTable enc_to_pos_lut;
|
||||
|
||||
LowpassFilter velocity_lowpass;
|
||||
PIDController pos_controller;
|
||||
PIDController velocity_controller;
|
||||
|
||||
float motor_pos = 0; // current motor position
|
||||
float motor_pos_prev = 0; // previous motor position
|
||||
float pos_error = 0; // current position error as computed by upate()
|
||||
float velocity = 0; // current velocity estimate
|
||||
|
||||
float motorpos_to_field_angle = 0; // conversion factor derived from pole pair count
|
||||
float motor_current_amplitude = 0.5f;
|
||||
};
|
||||
|
||||
//*** FUNCTIONS **************************************************************/
|
||||
|
||||
bool build_motor_to_enc_angle_lut(
|
||||
LookupTable& lut,
|
||||
ServoController& servo_controller,
|
||||
float min_motor_angle,
|
||||
float max_motor_angle,
|
||||
size_t size);
|
||||
133
firmware/MotionControllerRP/src/utilities/fp_math.h
Normal file
133
firmware/MotionControllerRP/src/utilities/fp_math.h
Normal file
|
|
@ -0,0 +1,133 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <limits>
|
||||
#include "math_constants.h"
|
||||
|
||||
// #define CHECK_FP_MATH_ERRORS
|
||||
#ifdef CHECK_FP_MATH_ERRORS
|
||||
#include "Arduino.h"
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief A header-only utility class for fixed-point arithmetic operations.
|
||||
*
|
||||
* This class provides methods to convert between floating-point and fixed-point,
|
||||
* and perform common fixed-point arithmetic (multiplication, division, reciprocal).
|
||||
* It uses a specified Q-format (number of fractional bits) for all operations.
|
||||
*/
|
||||
class FPMath {
|
||||
public:
|
||||
// constants
|
||||
const int32_t FP_ONE; // Represents the value 1.0 in fixed-point (1 << q)
|
||||
const int32_t FP_PI;
|
||||
const int32_t FP_TWO_PI;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for FPMath.
|
||||
* @param q_format The number of fractional bits for fixed-point representation.
|
||||
* A Q-format of 'q' means numbers are stored as N * 2^q.
|
||||
* For example, q=16 means 16 fractional bits.
|
||||
*/
|
||||
explicit FPMath(uint8_t q_format)
|
||||
: q(q_format),
|
||||
FP_ONE(int32_t(1) << q_format),
|
||||
US_TO_MSFP(FP_ONE/1000),
|
||||
FP_TO_FLOAT(1.0f/(int32_t(1)<<q_format)),
|
||||
FP_PI(to_fixpoint(Constants::PI_F)),
|
||||
FP_TWO_PI(to_fixpoint(Constants::TWO_PI_F))
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
inline uint8_t get_qformat() {
|
||||
return q;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Converts a floating-point value to its fixed-point representation.
|
||||
* @param v The floating-point value to convert.
|
||||
* @return The fixed-point representation of 'v'.
|
||||
*/
|
||||
inline int32_t to_fixpoint(float v) const {
|
||||
return static_cast<int32_t>(v * FP_ONE);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Converts a fixed-point value back to its floating-point representation.
|
||||
* @param v The fixed-point value to convert.
|
||||
* @return The floating-point representation of 'v'.
|
||||
*/
|
||||
inline float from_fixpoint(int32_t v) const {
|
||||
return static_cast<float>(v) * FP_TO_FLOAT;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Converts a duration in microseconds (uint32_t) to a fixed-point
|
||||
* representation in milliseconds.
|
||||
* @param dt_us The duration in microseconds, in normal integer format.
|
||||
* @return The duration in milliseconds, in fixed-point format.
|
||||
*/
|
||||
inline int32_t duration_us_to_ms(uint32_t dt_us) const {
|
||||
return static_cast<int32_t>(dt_us * US_TO_MSFP);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Performs fixed-point multiplication.
|
||||
* @param a First operand in fixed-point (Q-format).
|
||||
* @param b Second operand in fixed-point (Q-format).
|
||||
* @return Result of a * b in fixed-point (Q-format maintained).
|
||||
* Uses 64-bit intermediate multiplication to prevent overflow.
|
||||
*/
|
||||
inline int32_t mul(int32_t a, int32_t b) const {
|
||||
// Multiplying two Qx numbers results in a Q(2x) number.
|
||||
// Shifting right by 'q' converts it back to Qx.
|
||||
// Use int64_t for intermediate product to prevent overflow.
|
||||
// be aware that the compiler needs to keep the sign bit untouched
|
||||
int64_t product = static_cast<int64_t>(a) * b;
|
||||
int64_t shifted = product >> q;
|
||||
#ifdef CHECK_FP_MATH_ERRORS
|
||||
if (shifted > INT32_MAX) on_overflow_error();
|
||||
if (shifted < INT32_MIN) on_overflow_error();
|
||||
#endif
|
||||
return static_cast<int32_t>(shifted);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Performs fixed-point division.
|
||||
* @param a Numerator in fixed-point (Q-format).
|
||||
* @param b Denominator in fixed-point (Q-format).
|
||||
* @return Result of a / b in fixed-point (Q-format maintained).
|
||||
* Handles division by zero by saturating the result.
|
||||
*/
|
||||
inline int32_t div(int32_t a, int32_t b) const {
|
||||
if (b == 0) {
|
||||
// Handle division by zero: return saturation value.
|
||||
return (a >= 0) ? std::numeric_limits<int32_t>::max() : std::numeric_limits<int32_t>::min();
|
||||
}
|
||||
// To maintain Qx precision after division, multiply numerator by FP_ONE (2^q) before dividing.
|
||||
// Use int64_t for intermediate product to prevent overflow.
|
||||
return static_cast<int32_t>((static_cast<int64_t>(a) * FP_ONE) / b);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculates the reciprocal (1 / a) in fixed-point.
|
||||
* @param a The operand in fixed-point (Q-format).
|
||||
* @return The reciprocal of 'a' in fixed-point (Q-format).
|
||||
*/
|
||||
inline int32_t one_over(int32_t a) const {
|
||||
return div(FP_ONE, a);
|
||||
}
|
||||
|
||||
#ifdef CHECK_FP_MATH_ERRORS
|
||||
inline void on_overflow_error() const {
|
||||
Serial.printf("fp_math overflow error:");
|
||||
}
|
||||
#endif
|
||||
|
||||
private:
|
||||
const uint8_t q; // Number of fractional bits for fixed-point
|
||||
const int32_t US_TO_MSFP; // Pre-calculated fixed-point value for 1/1000 for us to ms conversion
|
||||
const float FP_TO_FLOAT;
|
||||
};
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
class FrequencyCounter {
|
||||
public:
|
||||
FrequencyCounter(uint32_t num_samples) : num_samples(num_samples), sum(0), count(0), freq(0) {}
|
||||
|
||||
void update(float dt) {
|
||||
sum += dt;
|
||||
if (++count >= num_samples && sum) {
|
||||
freq = num_samples / sum;
|
||||
sum = 0;
|
||||
count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t get() const { return freq; }
|
||||
|
||||
private:
|
||||
float num_samples, count;
|
||||
float sum;
|
||||
float freq;
|
||||
};
|
||||
80
firmware/MotionControllerRP/src/utilities/logging.cpp
Normal file
80
firmware/MotionControllerRP/src/utilities/logging.cpp
Normal file
|
|
@ -0,0 +1,80 @@
|
|||
#include "logging.h"
|
||||
#include "Arduino.h"
|
||||
#include <stdarg.h>
|
||||
|
||||
Logger& Logger::instance() {
|
||||
static Logger logger;
|
||||
return logger;
|
||||
}
|
||||
|
||||
void Logger::begin(unsigned long baudrate, bool wait_for_connection) {
|
||||
Serial.begin(baudrate);
|
||||
if(wait_for_connection)
|
||||
while(!Serial);
|
||||
}
|
||||
|
||||
void Logger::set_level(ELogLevel level) {
|
||||
current_level = level;
|
||||
}
|
||||
|
||||
void Logger::debug(const char* fmt, ...) {
|
||||
if (current_level <= ELogLevel::DEBUG) {
|
||||
va_list args;
|
||||
va_start(args, fmt);
|
||||
log(ELogLevel::DEBUG, fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
}
|
||||
|
||||
void Logger::info(const char* fmt, ...) {
|
||||
if (current_level <= ELogLevel::INFO) {
|
||||
va_list args;
|
||||
va_start(args, fmt);
|
||||
log(ELogLevel::INFO, fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
}
|
||||
|
||||
void Logger::warn(const char* fmt, ...) {
|
||||
if (current_level <= ELogLevel::WARN) {
|
||||
va_list args;
|
||||
va_start(args, fmt);
|
||||
log(ELogLevel::WARN, fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
}
|
||||
|
||||
void Logger::error(const char* fmt, ...) {
|
||||
if (current_level <= ELogLevel::ERROR) {
|
||||
va_list args;
|
||||
va_start(args, fmt);
|
||||
log(ELogLevel::ERROR, fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
}
|
||||
|
||||
void Logger::log(ELogLevel level, const char* fmt, va_list args) {
|
||||
char buf[128]; // Adjust size as needed
|
||||
vsnprintf(buf, sizeof(buf), fmt, args);
|
||||
|
||||
Serial.print(log_prefix(level));
|
||||
Serial.println(buf);
|
||||
}
|
||||
|
||||
const char* Logger::log_prefix(ELogLevel level) {
|
||||
switch (level) {
|
||||
case ELogLevel::DEBUG: return "[DEBUG] ";
|
||||
case ELogLevel::INFO: return "";
|
||||
case ELogLevel::WARN: return "[WARNING] ";
|
||||
case ELogLevel::ERROR: return "[ERROR] ";
|
||||
default: return "";
|
||||
}
|
||||
}
|
||||
|
||||
void error_trap(const char* message) {
|
||||
while(true) {
|
||||
sleep_ms(1000);
|
||||
if(message != nullptr)
|
||||
LOG_ERROR(message);
|
||||
}
|
||||
}
|
||||
50
firmware/MotionControllerRP/src/utilities/logging.h
Normal file
50
firmware/MotionControllerRP/src/utilities/logging.h
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
//*** MACRO *****************************************************************************
|
||||
|
||||
#define LOG_DEBUG(...) Logger::instance().debug(__VA_ARGS__)
|
||||
#define LOG_INFO(...) Logger::instance().info(__VA_ARGS__)
|
||||
#define LOG_WARNING(...) Logger::instance().warn(__VA_ARGS__)
|
||||
#define LOG_ERROR(...) Logger::instance().error(__VA_ARGS__)
|
||||
|
||||
//*** ENUM ******************************************************************************
|
||||
|
||||
enum class ELogLevel {
|
||||
DEBUG,
|
||||
INFO,
|
||||
WARN,
|
||||
ERROR,
|
||||
NONE
|
||||
};
|
||||
|
||||
//*** CLASS *****************************************************************************
|
||||
|
||||
class Logger {
|
||||
public:
|
||||
static Logger& instance();
|
||||
|
||||
void begin(unsigned long baudrate = 115200, bool wait_for_connection=false);
|
||||
void set_level(ELogLevel level);
|
||||
|
||||
void debug(const char* fmt, ...);
|
||||
void info(const char* fmt, ...);
|
||||
void warn(const char* fmt, ...);
|
||||
void error(const char* fmt, ...);
|
||||
|
||||
private:
|
||||
Logger() = default;
|
||||
Logger(const Logger&) = delete;
|
||||
Logger& operator=(const Logger&) = delete;
|
||||
|
||||
void log(ELogLevel level, const char* fmt, va_list args);
|
||||
const char* log_prefix(ELogLevel level);
|
||||
|
||||
ELogLevel current_level = ELogLevel::DEBUG;
|
||||
};
|
||||
|
||||
//*** FUNCTION **************************************************************************
|
||||
|
||||
void error_trap(const char* message="");
|
||||
|
||||
208
firmware/MotionControllerRP/src/utilities/math3d.h
Normal file
208
firmware/MotionControllerRP/src/utilities/math3d.h
Normal file
|
|
@ -0,0 +1,208 @@
|
|||
#pragma once
|
||||
#include <cmath>
|
||||
|
||||
//--- Vec3F -----------------------------------------------------------------------------
|
||||
|
||||
class Vec3F {
|
||||
public:
|
||||
Vec3F() : x(0.0f), y(0.0f), z(0.0f) {}
|
||||
Vec3F(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
|
||||
|
||||
Vec3F operator+(const Vec3F& v) const { return Vec3F(x + v.x, y + v.y, z + v.z); }
|
||||
Vec3F operator-(const Vec3F& v) const { return Vec3F(x - v.x, y - v.y, z - v.z); }
|
||||
Vec3F operator*(float s) const { return Vec3F(x * s, y * s, z * s); }
|
||||
Vec3F operator/(float s) const { float t = 1.0f/s; return Vec3F(x*t, y*t, z*t); }
|
||||
|
||||
float dot(const Vec3F& v) const { return x * v.x + y * v.y + z * v.z; }
|
||||
float length() const { return std::sqrt(x * x + y * y + z * z); }
|
||||
float sqr_length() const { return x * x + y * y + z * z; }
|
||||
Vec3F cross(const Vec3F& v) const {
|
||||
return Vec3F(
|
||||
y * v.z - z * v.y,
|
||||
z * v.x - x * v.z,
|
||||
x * v.y - y * v.x
|
||||
);
|
||||
}
|
||||
|
||||
Vec3F normalized() const {
|
||||
float len = length();
|
||||
return len > 0.0f ? (*this / len) : Vec3F(0, 0, 0);
|
||||
}
|
||||
|
||||
public:
|
||||
float x, y, z;
|
||||
};
|
||||
|
||||
//--- QuaternionF -----------------------------------------------------------------------
|
||||
|
||||
class QuaternionF {
|
||||
public:
|
||||
QuaternionF() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {}
|
||||
QuaternionF(float w_, float x_, float y_, float z_) : w(w_), x(x_), y(y_), z(z_) {}
|
||||
|
||||
static QuaternionF from_axis_angle(const Vec3F& axis, float angle_rad) {
|
||||
Vec3F naxis = axis.normalized();
|
||||
float half_angle = 0.5f * angle_rad;
|
||||
float s = std::sin(half_angle);
|
||||
return QuaternionF(std::cos(half_angle), naxis.x * s, naxis.y * s, naxis.z * s);
|
||||
}
|
||||
|
||||
static QuaternionF from_rot_vec(const Vec3F& rot_vec) {
|
||||
float length = rot_vec.length();
|
||||
Vec3F naxis = rot_vec/length;
|
||||
float half_angle = 0.5f * length;
|
||||
float s = std::sin(half_angle);
|
||||
return QuaternionF(std::cos(half_angle), naxis.x * s, naxis.y * s, naxis.z * s);
|
||||
}
|
||||
|
||||
void to_axis_angle(Vec3F& axis, float& angle) const {
|
||||
angle = 2.0f * std::acos(std::fmax(-1.0f, std::fmin(1.0f, w)));
|
||||
|
||||
float sin_half_angle = std::sqrt(1.0f - w * w);
|
||||
|
||||
if (sin_half_angle < 1e-6f)
|
||||
axis = Vec3F(1.0f, 0.0f, 0.0f);
|
||||
else
|
||||
axis = Vec3F(x, y, z) / sin_half_angle;
|
||||
}
|
||||
|
||||
float angle() {
|
||||
// Clamp to avoid domain errors due to floating point
|
||||
float angle = 2.0f * std::acos(std::fmax(-1.0f, std::fmin(1.0f, w)));
|
||||
return angle; // In radians, range: [0, π]
|
||||
}
|
||||
|
||||
// return inverse of a normalized quaternion
|
||||
QuaternionF normalized_inverse() const {
|
||||
return QuaternionF(w, -x, -y, -z);
|
||||
}
|
||||
|
||||
QuaternionF operator*(const QuaternionF& q) const {
|
||||
return QuaternionF(
|
||||
w * q.w - x * q.x - y * q.y - z * q.z,
|
||||
w * q.x + x * q.w + y * q.z - z * q.y,
|
||||
w * q.y - x * q.z + y * q.w + z * q.x,
|
||||
w * q.z + x * q.y - y * q.x + z * q.w
|
||||
);
|
||||
}
|
||||
|
||||
Vec3F rotate(const Vec3F& v) const {
|
||||
Vec3F qvec(x, y, z);
|
||||
Vec3F t = qvec.cross(v) * 2.0f;
|
||||
return v + t * w + qvec.cross(t);
|
||||
}
|
||||
|
||||
QuaternionF normalized() const {
|
||||
float norm = std::sqrt(w * w + x * x + y * y + z * z);
|
||||
return norm > 0.0f ? QuaternionF(w / norm, x / norm, y / norm, z / norm) : QuaternionF();
|
||||
}
|
||||
|
||||
// spherical linear interpolation, both input quaternions must be normalized
|
||||
QuaternionF slerp(const QuaternionF& other, float t) const {
|
||||
float dot = w * other.w + x * other.x + y * other.y + z * other.z;
|
||||
QuaternionF q2 = other;
|
||||
|
||||
if (dot < 0.0f) {
|
||||
dot = -dot;
|
||||
q2 = QuaternionF(-q2.w, -q2.x, -q2.y, -q2.z);
|
||||
}
|
||||
|
||||
const float DOT_THRESHOLD = 0.9995f;
|
||||
if (dot > DOT_THRESHOLD) {
|
||||
// LERP + normalize for nearly identical quaternions
|
||||
QuaternionF result(
|
||||
w + t * (q2.w - w),
|
||||
x + t * (q2.x - x),
|
||||
y + t * (q2.y - y),
|
||||
z + t * (q2.z - z)
|
||||
);
|
||||
return result.normalized(); // still necessary for lerp path
|
||||
}
|
||||
|
||||
float theta_0 = std::acos(dot);
|
||||
float theta = theta_0 * t;
|
||||
float sin_theta = std::sin(theta);
|
||||
float sin_theta_0 = std::sin(theta_0);
|
||||
|
||||
float s1 = std::cos(theta) - dot * sin_theta / sin_theta_0;
|
||||
float s2 = sin_theta / sin_theta_0;
|
||||
|
||||
return QuaternionF(
|
||||
s1 * w + s2 * q2.w,
|
||||
s1 * x + s2 * q2.x,
|
||||
s1 * y + s2 * q2.y,
|
||||
s1 * z + s2 * q2.z
|
||||
);
|
||||
}
|
||||
|
||||
public:
|
||||
float w, x, y, z;
|
||||
};
|
||||
|
||||
//--- Pose6DF ---------------------------------------------------------------------------
|
||||
|
||||
class Pose6DF {
|
||||
public:
|
||||
Pose6DF() : translation(), rotation() {}
|
||||
Pose6DF(const Vec3F& t, const QuaternionF& r) : translation(t), rotation(r.normalized()) {}
|
||||
|
||||
/// Transform a point from local to world space
|
||||
Vec3F transformPoint(const Vec3F& localPoint) const {
|
||||
return rotation.rotate(localPoint) + translation;
|
||||
}
|
||||
|
||||
/// Combine with another pose (this * other)
|
||||
Pose6DF operator*(const Pose6DF& other) const {
|
||||
Vec3F newTranslation = transformPoint(other.translation);
|
||||
QuaternionF newRotation = (rotation * other.rotation).normalized();
|
||||
return Pose6DF(newTranslation, newRotation);
|
||||
}
|
||||
|
||||
/// Invert this pose
|
||||
Pose6DF inverse() const {
|
||||
QuaternionF inv_rot = rotation.normalized_inverse();
|
||||
Vec3F inv_trans = inv_rot.rotate(translation * -1.0f);
|
||||
return Pose6DF(inv_trans, inv_rot);
|
||||
}
|
||||
|
||||
// Linearly interpolate between two poses
|
||||
static Pose6DF lerp(const Pose6DF& a, const Pose6DF& b, float t) {
|
||||
// Linear interpolation for translation
|
||||
Vec3F translation = a.translation * (1.0f - t) + b.translation * t;
|
||||
|
||||
// Spherical linear interpolation for rotation
|
||||
QuaternionF rotation = a.rotation.slerp(b.rotation, t).normalized();
|
||||
|
||||
return Pose6DF(translation, rotation);
|
||||
}
|
||||
|
||||
public:
|
||||
Vec3F translation;
|
||||
QuaternionF rotation;
|
||||
};
|
||||
|
||||
//--- LinearAngular ---------------------------------------------------------------------
|
||||
|
||||
class LinearAngular {
|
||||
public:
|
||||
LinearAngular(float l = 1.0f, float a = 1.0f) : linear(l), angular(a) {};
|
||||
|
||||
LinearAngular operator+(const LinearAngular& other) const {
|
||||
return { linear + other.linear, angular + other.angular };
|
||||
}
|
||||
LinearAngular operator-(const LinearAngular& other) const {
|
||||
return { linear - other.linear, angular - other.angular };
|
||||
}
|
||||
LinearAngular operator*(float scalar) const {
|
||||
return { linear * scalar, angular * scalar };
|
||||
}
|
||||
LinearAngular operator*(const LinearAngular& other) const {
|
||||
return { linear * other.linear, angular * other.angular };
|
||||
}
|
||||
|
||||
public:
|
||||
float linear = 1.0f; // mm/s
|
||||
float angular = 1.0f; // rad/s
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
namespace Constants {
|
||||
constexpr float PI_F = 3.1415927f;
|
||||
constexpr float TWO_PI_F = 6.2831855f;
|
||||
constexpr float RAD2DEG = 57.29577951308232f;
|
||||
constexpr float DEG2RAD = 0.017453292519943295f;
|
||||
}
|
||||
46
firmware/MotionControllerRP/src/utilities/ringbuffer.h
Normal file
46
firmware/MotionControllerRP/src/utilities/ringbuffer.h
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
template<typename T, int N>
|
||||
class RingBuffer {
|
||||
public:
|
||||
RingBuffer() : head(0), tail(0), count(0) {}
|
||||
|
||||
T* push(const T& value) {
|
||||
if (count == N) return nullptr; // Full
|
||||
buffer[head] = value;
|
||||
T* ptr = &buffer[head];
|
||||
head = (head + 1) % N;
|
||||
++count;
|
||||
return ptr;
|
||||
}
|
||||
|
||||
bool pop(T& value) {
|
||||
if (count == 0) return false; // Empty
|
||||
value = buffer[tail];
|
||||
tail = (tail + 1) % N;
|
||||
--count;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pop() {
|
||||
if (count == 0) return false; // Empty
|
||||
tail = (tail + 1) % N;
|
||||
--count;
|
||||
return true;
|
||||
}
|
||||
|
||||
T* peek() {
|
||||
if (count == 0) return nullptr;
|
||||
return &(buffer[tail]);
|
||||
}
|
||||
|
||||
bool empty() const { return count == 0; }
|
||||
bool full() const { return count == N; }
|
||||
int size() const { return count; }
|
||||
int free_item_count() const { return N-count; }
|
||||
|
||||
private:
|
||||
T buffer[N];
|
||||
volatile int head;
|
||||
volatile int tail;
|
||||
volatile int count;
|
||||
};
|
||||
71
firmware/MotionControllerRP/src/utilities/waveforms.cpp
Normal file
71
firmware/MotionControllerRP/src/utilities/waveforms.cpp
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
|
||||
#include "waveforms.h"
|
||||
#include "math_constants.h"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
float triangle_with_plateau(float x) {
|
||||
const float period = Constants::TWO_PI_F;
|
||||
const float segment = period * 0.16666666666f;
|
||||
float t = fmodf(x, period); // wrap x to [0, 2π)
|
||||
|
||||
if (t < segment) {
|
||||
return t / segment; // 0 → +1
|
||||
} else if (t < 2 * segment) {
|
||||
return 1.0f - (t - segment) / segment; // +1 → 0
|
||||
} else if (t < 3 * segment) {
|
||||
return 0.0f; // plateau
|
||||
} else if (t < 4 * segment) {
|
||||
return -(t - 3 * segment) / segment; // 0 → -1
|
||||
} else if (t < 5 * segment) {
|
||||
return -1.0f + (t - 4 * segment) / segment; // -1 → 0
|
||||
} else {
|
||||
return 0.0f; // plateau
|
||||
}
|
||||
}
|
||||
|
||||
float triangle_wave(float x) {
|
||||
const float period = Constants::TWO_PI_F;
|
||||
float t = fmodf(x, period);
|
||||
|
||||
// Normalize t to [0, 1)
|
||||
float phase = t / period;
|
||||
|
||||
// Scale to triangle shape in [-1, 1]
|
||||
if (phase < 0.25f)
|
||||
return 4.0f * phase; // 0 → +1
|
||||
else if (phase < 0.75f)
|
||||
return 2.0f - 4.0f * phase; // +1 → -1
|
||||
else
|
||||
return -4.0f + 4.0f * phase; // -1 → 0
|
||||
}
|
||||
|
||||
float trapezoidal_wave(float x, float plateau_fraction) {
|
||||
const float two_pi = 2.0f * M_PI;
|
||||
|
||||
// Clamp plateau_fraction to valid range
|
||||
plateau_fraction = std::clamp(plateau_fraction, 0.0f, 0.4999f);
|
||||
|
||||
// Calculate segment widths
|
||||
float plateau_width = plateau_fraction * two_pi;
|
||||
float ramp_width = (two_pi - 2 * plateau_width) / 2.0f;
|
||||
|
||||
// Wrap x into [0, 2π)
|
||||
float phase = fmodf(x, two_pi);
|
||||
if (phase < 0.0f) phase += two_pi;
|
||||
|
||||
if (phase < plateau_width) {
|
||||
// Bottom plateau
|
||||
return -1.0f;
|
||||
} else if (phase < plateau_width + ramp_width) {
|
||||
// Rising edge
|
||||
return -1.0f + 2.0f * (phase - plateau_width) / ramp_width;
|
||||
} else if (phase < plateau_width + ramp_width + plateau_width) {
|
||||
// Top plateau
|
||||
return 1.0f;
|
||||
} else {
|
||||
// Falling edge
|
||||
return 1.0f - 2.0f * (phase - (2 * plateau_width + ramp_width)) / ramp_width;
|
||||
}
|
||||
}
|
||||
|
||||
4
firmware/MotionControllerRP/src/utilities/waveforms.h
Normal file
4
firmware/MotionControllerRP/src/utilities/waveforms.h
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
|
||||
float triangle_with_plateau(float x);
|
||||
float triangle_wave(float x);
|
||||
float trapezoidal_wave(float x, float plateau_fraction = 0.2f);
|
||||
Loading…
Add table
Add a link
Reference in a new issue