fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Version v1.0.1:

Browse source 
* Improved homing (parallel homing support, better repeatability, better geometric reference point)
 * Improved joint calibration procedure
 * Calibration data can now be stored persistently on the flash memory (no repeated calibration required)
 * Improved logging
 * added PythonAPI to control device easily

New G-Code commands:
 * Enable/Disable motors command, including pose recovery from current position on motor enable
 * Dedicated joint calibration command with save to flash option
 * Set pose command to directly set a target pose for the servo loops, bypassing the motion controller (good for real-time control)
This commit is contained in:
0x23 2025-09-19 09:24:56 +02:00
parent 2cf353e7fc
commit d9888ef369
27 changed files with 1723 additions and 784 deletions
Download patch file Download diff file Expand all files Collapse all files

93
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
View file

@ -3,83 +3,92 @@
// being distributed under the MIT liscence as well. Thank you SimpleFOC !
// --------------------------------------------------------------------------------------
#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);
// 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
// nop
}
MT6835Encoder::~MT6835Encoder() {
// nop
// 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
}
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);
set_rotation_direction(0); // needs to be set, otherwise might be random
set_bandwidth(bandwidth);
set_hysteresis(hysteresis);
last_raw_angle = 0;
abs_raw_angle = 0;
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;
}
void MT6835Encoder::reset_abs_angle_period() {
abs_raw_angle %= MT6835_CPR;
if (abs_raw_angle < 0)
abs_raw_angle += MT6835_CPR;
}
float MT6835Encoder::read_abs_angle() {
int32_t raw_angle = read_abs_angle_raw();
return raw_angle * RAW_TO_ANGLE;
int32_t raw_angle = read_abs_angle_raw();
return raw_angle * RAW_TO_ANGLE;
}
MT6835Encoder::AbsRawAngleType MT6835Encoder::read_abs_angle_raw() {
uint8_t data[6] = {0};
data[0] = MT6835_OP_ANGLE << 4;
data[1] = MT6835_REG_ANGLE1;
// rest zero
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();
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
}
}
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);
return update_abs_raw_angle(raw_angle);
}
MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() {
MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() const {
return abs_raw_angle;
}
float MT6835Encoder::get_last_abs_angle() const {
return abs_raw_angle * RAW_TO_ANGLE;
}
int32_t MT6835Encoder::get_rawcounts_per_rev() {
return MT6835_CPR;
}

5
firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
View file

@ -147,10 +147,11 @@ class 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
void reset_abs_angle_period(); // Brings abs angle into [0..2pi)
float read_abs_angle(); // returns the absolute angle in radians
float get_last_abs_angle() const; // returns the last read abs angle
AbsRawAngleType read_abs_angle_raw(); // returns the absolute angle in raw counts
AbsRawAngleType get_last_abs_raw_angle(); // returns the last read abs raw angle
AbsRawAngleType get_last_abs_raw_angle() const; // returns the last read abs raw angle
int32_t get_rawcounts_per_rev(); // returns the number of raw counts per revolution

67
firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
View file

@ -6,12 +6,14 @@
// --------------------------------------------------------------------------------------
#include "TB6612_motor_driver.h"
#include "utilities/logging.h"
#include <math.h>
#include <algorithm>
#include "hardware/pwm.h"
#include "hardware/gpio.h"
#include "hardware/clocks.h"
#include "pico/time.h"
// Helper rounding function
int32_t round_int32(float val) {
@ -33,7 +35,7 @@ TB6612MotorDriver::TB6612MotorDriver(
pwm_freq(pwm_freq), pwm_resolution(pwm_resolution)
{
max_pwm = (1 << pwm_resolution) - 1;
amplitude = 0.1f * max_pwm; // Default to 10% amplitude
set_amplitude(0.0f, false);
}
void init_output_pin(uint8_t pin, bool value) {
@ -46,17 +48,11 @@ 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
// 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);
@ -76,6 +72,8 @@ void TB6612MotorDriver::begin() {
setup_pwm_pin(pin_neg_a);
setup_pwm_pin(pin_pos_b);
setup_pwm_pin(pin_neg_b);
disable();
}
void TB6612MotorDriver::enable() {
@ -90,18 +88,63 @@ void TB6612MotorDriver::disable() {
void TB6612MotorDriver::set_amplitude(float amplitude, bool immediate_update) {
amplitude = std::clamp(amplitude, 0.0f, 1.0f);
TB6612MotorDriver::amplitude = amplitude * max_pwm;
TB6612MotorDriver::amplitude = amplitude;
TB6612MotorDriver::amplitude_raw = amplitude * max_pwm;
if(immediate_update)
set_field_angle(field_angle);
}
void TB6612MotorDriver::set_amplitude_smooth(float amplitude, int ramp_time_ms) {
amplitude = std::clamp(amplitude, 0.0f, 1.0f);
float start = TB6612MotorDriver::amplitude;
int step_time = 10;
int steps = std::max(1, ramp_time_ms / step_time);
for (int i = 1; i <= steps; ++i) {
float t = float(i) / steps;
set_amplitude(start + t * (amplitude - start), true);
sleep_ms(step_time);
}
}
float TB6612MotorDriver::get_amplitude() const {
return amplitude;
}
void TB6612MotorDriver::rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step) {
float start = field_angle;
float rad_per_µs = (delta_angle >= 0.0f) ? rad_per_s*1e-6f : -rad_per_s*1e-6f;
// Determine how long the movement should take (in microseconds)
uint64_t duration_us = (uint64_t)(fabs(delta_angle / rad_per_s) * 1e6f);
uint64_t start_time_us = time_us_64();
while (true) {
uint64_t elapsed_us = time_us_64() - start_time_us;
if (elapsed_us >= duration_us)
break;
// Update the field
set_field_angle(start + rad_per_µs * elapsed_us);
if(on_step)
on_step();
sleep_us(500);
}
// Ensure final angle is reached
set_field_angle(start + delta_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));
set_pwm(pin_pos_a, pin_neg_a, round_int32(sin_a * amplitude_raw));
set_pwm(pin_pos_b, pin_neg_b, round_int32(cos_a * amplitude_raw));
}
float TB6612MotorDriver::get_field_angle() {

11
firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.h
View file

@ -8,6 +8,7 @@
#pragma once
#include <stdint.h>
#include <functional>
// #include <Arduino.h>
class TB6612MotorDriver {
@ -27,6 +28,12 @@ class TB6612MotorDriver {
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
void set_amplitude_smooth(float amplitude, int ramp_time_ms);
float get_amplitude() const;
// Rotate the magnetic field by the given angle delta.
// For longer moves, use on_step callback to update encoders.
void rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step);
private:
void set_pwm(uint8_t ch_pos, uint8_t ch_neg, int32_t value);
@ -39,6 +46,8 @@ class TB6612MotorDriver {
uint16_t pwm_freq;
uint8_t pwm_resolution;
uint16_t max_pwm;
float amplitude; // scaled to 0–max_pwm
float amplitude_raw=0.0f; // scaled to 0–max_pwm
float amplitude=0.0f; // scaled to 0–1
float field_angle;
};

67
firmware/MotionControllerRP/src/hw_config.h
View file

@ -5,39 +5,16 @@
#define MOTOR2_POLE_PAIRS 100
#define MOTOR3_POLE_PAIRS 100
// #define SINGLE_AXIS_BOARD
#ifdef SINGLE_AXIS_BOARD
// Single Axis Board
#define PIN_BUILTIN_LED 16
#define PIN_USER_BUTTON 24
#define CALIBRATION_RANGE 95 // degrees from home position
#define CALIBRATION_FIELD_VELOCITY 40.0f
#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 HOMING_VELOCITY 1.0f // rad per s
#define HOMING_CURRENT 0.15f // range 0..1
#define HOMING_FINISH_POS 0.5f // in rad
#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
#define SINGLE_AXIS_BOARD
#ifndef SINGLE_AXIS_BOARD
// Pins for 3Axis Board
#define PIN_BUILTIN_LED 23
#define PIN_USER_BUTTON 24
@ -68,6 +45,36 @@
#define PIN_ENCODER_MISO 0
#define PIN_ENCODER_MOSI 3
#else
// 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
#endif
// test setup

4
firmware/MotionControllerRP/src/kinemtaic_models/kinematic_model_delta3d.cpp
View file

@ -109,10 +109,6 @@ bool KinematicModel_Delta3D::inverse(const Pose6DF& pose, float* joint_positions
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;
}

226
firmware/MotionControllerRP/src/main.cpp
View file

@ -19,8 +19,9 @@
#include "utilities/logging.h"
#include "utilities/frequency_counter.h"
#include "kinemtaic_models/kinematic_model_delta3d.h"
#include "version.h"
#include "hw_config.h"
#include "LittleFS.h"
//*** GLOBALS ***************************************************************************
@ -67,8 +68,9 @@ void overclock() {
}
void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
return;
strip.neoPixelSetValue(0, r, g, b, false);
delayMicroseconds(1000);
delayMicroseconds(2000);
strip.neoPixelShow();
}
@ -92,7 +94,7 @@ void main_core0() {
}
void main_core1() {
LOG_INFO("starting servo controll loops on core 1...");
LOG_INFO("Starting servo controll loops on core 1...");
uint64_t last_time = time_us_64();
while(true) {
@ -101,12 +103,16 @@ void main_core1() {
float dt = float(time_us - last_time)*1e-6f;
last_time = time_us;
// limit time delta
dt = std::min(dt, 0.0001f);
// update servo loops
robot.update_servo_controllers(dt);
}
}
void setup() {
led_blink(0, 20, 0, 1, 4000/3);
led_blink(0, 0, 30, 3, 100);
// stdio_init_all(); // Initializes USB or UART stdio
overclock();
// Serial.begin(921600);
@ -117,18 +123,29 @@ void setup() {
// 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("Open Micro Stage Firmware: %s", FIRMWARE_VERSION);
LOG_INFO("System clock: %i Mhz", int32_t(clock_get_hz(clk_sys))/1000/1000);
LOG_INFO("initializing robot...");
// LittleFS.format();
if (!LittleFS.begin()) {
LOG_ERROR("Mounting filesystem failed");
} else {
FSInfo fs_info;
LittleFS.info(fs_info);
LOG_INFO("Mounting filesystem successfully [%i/%i bytes used]",
(int)fs_info.usedBytes, (int)fs_info.totalBytes);
}
LOG_INFO("Initializing device...");
robot.init();
LOG_INFO("homing axes...");
robot.home();
multicore_launch_core1(&main_core1);
sleep_ms(100);
set_led_color(0, 10, 0);
LOG_INFO("initialization finished...");
set_led_color(0, 20, 0);
LOG_INFO("Initialization finished");
LOG_INFO(" ");
return;
@ -140,193 +157,8 @@ void setup() {
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);
// }
}

7
firmware/MotionControllerRP/src/main.h
View file

@ -1,6 +1 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------

14
firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
View file

@ -22,16 +22,17 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
// increment time counter
current_time += dt;
// check if end of current path segment exceeded and if so, fetch next one
// check if end of current path segments end time is exceeded and if so, fetch next one
bool queue_empty = false;
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);
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();
@ -40,6 +41,13 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
if(!current_path_segment.is_initialized())
return false;
// if queue_empty is true the current path segment is finished AND no more
// pending segments are in the queue. In that case the current path segment
// is disabled for the upcoming iterations. However the current iteration
// will still provide the exact end position to the caller and return true.
if(queue_empty)
current_path_segment.initialized = false;
// evaluate path segment
current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
return true;

1
firmware/MotionControllerRP/src/motion_control/motion_controller.h
View file

@ -30,4 +30,5 @@ class MotionController {
float current_time;
JointSpacePathSegment current_path_segment;
bool current_segment_finished;
};

536
firmware/MotionControllerRP/src/robot.cpp
View file

@ -5,10 +5,19 @@
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#include <LittleFS.h>
#include "robot.h"
#include "hw_config.h"
#include "utilities/logging.h"
#include "utilities/utilities.h"
#include "kinemtaic_models/kinematic_model_delta3d.h"
#include "servo_control/homing_controller.h"
#include "servo_control/actuator_calibration.h"
#include "pico/multicore.h"
#include "version.h"
constexpr int SPINLOCK_ID_SHARED_DATA = 0;
constexpr int SPINLOCK_ID_JOINTS = 1;
//*** FUNCTION **************************************************************************
@ -22,8 +31,8 @@ bool startswith(const std::string& str, const std::string& prefix) {
//--- RobotAxis -------------------------------------------------------------------------
RobotJoint::RobotJoint(MT6835Encoder* encoder,
TB6612MotorDriver* motor_driver,
int pole_pairs)
TB6612MotorDriver* motor_driver,
int pole_pairs)
{
RobotJoint::encoder = encoder;
RobotJoint::motor_driver = motor_driver;
@ -41,33 +50,47 @@ RobotJoint::~RobotJoint() {
encoder = nullptr;
}
void RobotJoint::init() {
void RobotJoint::init(int joint_idx) {
RobotJoint::joint_idx = joint_idx;
encoder->init(0x5, 0x4);
servo_controller->init(0.5);
servo_controller->set_motor_enabled(false, false);
}
void RobotJoint::home() {
servo_controller->home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
position = servo_controller->get_position();
velocity = 0.0f;
}
bool RobotJoint::calibrate() {
LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
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);
HomingController homing_controller;
bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY,
360.0f*DEG_TO_RAD, HOMING_CURRENT);
if(homing_ok == false) {
LOG_ERROR("Joint-%i: Calibration failed due to unsuccessful homing sequence", joint_idx);
return false;
}
LOG_DEBUG(">finished");
// lut.print_to_log();
delay(200);
servo_controller->set_encoder_lut(lut);
// measure lookup tables
LookupTable encoder_raw_to_motor_pos_lut;
LookupTable motor_pos_to_field_angle_lut;
bool ok = measure_calibration_data(encoder_raw_to_motor_pos_lut,
motor_pos_to_field_angle_lut,
*servo_controller,
CALIBRATION_RANGE*DEG_TO_RAD,
CALIBRATION_FIELD_VELOCITY,
256);
if(!ok) {
LOG_ERROR("Joint-%i: calibrating failed", joint_idx);
return false;
}
servo_controller->set_enc_to_pos_lut(encoder_raw_to_motor_pos_lut);
servo_controller->set_pos_to_field_lut(motor_pos_to_field_angle_lut);
is_calibrated = true;
is_homed = true;
LOG_INFO("Joint-%i: calibrating joint successful.", joint_idx);
return true;
}
void RobotJoint::update(float dt, float one_over_dt) {
@ -79,18 +102,61 @@ void RobotJoint::update_target(float p, float v) {
velocity = v;
}
bool RobotJoint::load_calibration() {
std::string fn1 = calib_data_filename("enc_to_pos_lut").c_str();
std::string fn2 = calib_data_filename("pos_to_field_lut").c_str();
if(!LittleFS.exists(fn1.c_str()) || !LittleFS.exists(fn2.c_str())) {
LOG_WARNING("Joint-%i: Not all calibration files found. Run joint calibration with M56.", joint_idx);
return false;
}
LookupTable enc_to_pos_lut;
LookupTable pos_to_field_lut;
bool res = true;
res &= load_lut_from_file(enc_to_pos_lut, fn1.c_str());
res &= load_lut_from_file(pos_to_field_lut, fn2.c_str());
if(res == false)
return false;
servo_controller->set_enc_to_pos_lut(enc_to_pos_lut);
servo_controller->set_pos_to_field_lut(pos_to_field_lut);
is_calibrated = true;
LOG_INFO("Joint-%i: Encoder lookup tables loaded (size=%i,%i)",
joint_idx, enc_to_pos_lut.size(), pos_to_field_lut.size());
return true;
}
bool RobotJoint::store_calibration() {
bool res = true;
res &= save_lut_to_file(servo_controller->get_enc_to_pos_lut(),
calib_data_filename("enc_to_pos_lut").c_str());
res &= save_lut_to_file(servo_controller->get_pos_to_field_lut(),
calib_data_filename("pos_to_field_lut").c_str());
return res;
}
std::string RobotJoint::calib_data_filename(std::string data_name) const {
return std::string("joint")+std::to_string(joint_idx)+"_"+data_name+".dat";
}
//--- Robot -----------------------------------------------------------------------------
Robot::Robot(float path_segment_time_step) :
path_planner(nullptr, path_segment_time_step),
motion_controller(&path_planner),
servo_loop_frequency_counter(10000),
motion_controller_frequency_counter(1000)
motion_controller_frequency_counter(1000),
shared_data(SPINLOCK_ID_SHARED_DATA),
joints_spin_lock(spin_lock_instance(SPINLOCK_ID_JOINTS))
{
kinematic_model = new KinematicModel_Delta3D();
path_planner.set_kinematic_model(kinematic_model);
for(int i=0; i<3; i++)
for(int i=0; i<NUM_JOINTS; i++)
joints[i] = nullptr;
command_parser.set_command_processor(this);
@ -106,7 +172,7 @@ Robot::~Robot() {
if(kinematic_model != nullptr)
delete kinematic_model;
for(int i=0; i<3; i++) {
for(int i=0; i<NUM_JOINTS; i++) {
if(joints[i] != nullptr)
delete joints[i];
joints[i] = nullptr;
@ -147,8 +213,9 @@ void Robot::init() {
}
// initialize axes
for(int i=0; i<3; i++) {
joints[i]->init();
for(int i=0; i<NUM_JOINTS; i++) {
joints[i]->init(i);
joints[i]->load_calibration();
}
// setup timer for updating the motion controller (which evaluates joint space path
@ -160,26 +227,6 @@ void Robot::init() {
&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
@ -198,6 +245,7 @@ void Robot::update_command_parser() {
//LOG_INFO(">pos_error [µrad]: %f\n", pos_error*1e6);
//Serial.printf(">pos_error [µrad]: %f\n", pos_error*1e6);#
//LOG_INFO(">pos_x [mm]: %f", joints[1]->position);
//update_servo_controllers(0.01f);
}
/**
@ -252,8 +300,8 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
// 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];
robot->shared_data.joint_target_positions[i] = joint_positions[i];
robot->shared_data.joint_target_velocities[i] = joint_velocities[i];
}
spin_unlock_unsafe(robot->shared_data.lock);
}
@ -272,19 +320,193 @@ void Robot::update_servo_controllers(float 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]);
for(int i=0; i<3; i++) {
joints[i]->update_target(shared_data.joint_target_positions[i],
shared_data.joint_target_velocities[i]);
}
spin_unlock_unsafe(shared_data.lock);
// update servo loop for each axis
for(int i=0; i<3; i++) {
spin_lock_unsafe_blocking(joints_spin_lock);
for(int i=0; i<NUM_JOINTS; i++) {
joints[i]->update(dt, one_over_dt);
}
spin_unlock_unsafe(joints_spin_lock);
// update frequency counter
servo_loop_frequency_counter.update(dt);
}
void Robot::enable_servo_control(bool enable) {
// LOG_DEBUG(enable ? "Enable servo control" : "Disable servo sontrol");
// update servo loop for each axis
spin_lock_unsafe_blocking(joints_spin_lock);
for(int i=0; i<NUM_JOINTS; i++) {
bool en = joints[i]->is_homed && joints[i]->is_calibrated && enable;
LOG_DEBUG(en ? "Joint-%i: servo control enabled" : "Joint-%i: servo control disabled", i);
joints[i]->servo_controller->set_motor_update_enabled(en);
}
spin_unlock_unsafe(joints_spin_lock);
}
void Robot::set_pose(const Pose6DF& pose) {
// run inverse kinematic and compute joint positions
float joint_positions[NUM_JOINTS];
kinematic_model->inverse(pose, joint_positions);
while(true) {
// Attempt to acquire spinlock non-blocking and set new target data for the servo loops
if (spin_try_lock_unsafe(shared_data.lock)) {
for (int i = 0; i < NUM_JOINTS; i++) {
shared_data.joint_target_positions[i] = joint_positions[i];
shared_data.joint_target_velocities[i] = 0.0f;
// LOG_DEBUG("Joint-%i: set pose -> angle %f", i, joint_positions[i]);
}
spin_unlock_unsafe(shared_data.lock);
break;
}
}
current_pose = pose;
}
Pose6DF Robot::pose_from_joint_angles() {
// read joint positions from encoders
float joint_pos[NUM_JOINTS];
spin_lock_unsafe_blocking(joints_spin_lock);
for (int i = 0; i < NUM_JOINTS; i++) {
joint_pos[i] = joints[i]->servo_controller->read_position();
}
spin_unlock_unsafe(joints_spin_lock);
// run foreward kinematic model to retrieve pose from joint positions
Pose6DF pose;
bool ok = kinematic_model->foreward(joint_pos, pose);
if(ok == false)
LOG_ERROR("Foreward kinematic failed");
return pose;
}
bool Robot::check_all_joints_ready() {
bool all_ready = true;
for(int i=0; i<NUM_JOINTS; i++) {
all_ready &= joints[i]->is_calibrated && joints[i]->is_homed;
}
return all_ready;
}
bool Robot::home(uint8_t joint_mask) {
HomingController homing_controller[NUM_JOINTS];
LOG_INFO("homing...");
enable_servo_control(false);
// prevent servo loop updates from running during homing
spin_lock_unsafe_blocking(joints_spin_lock);
// initialize homing controllers
for(int i=0; i<NUM_JOINTS; i++) {
// only start requested joints
if(((joint_mask>>i)&1) == 0) continue;
LOG_DEBUG("start homing axis %i", i);
homing_controller[i].start(joints[i]->servo_controller,
-HOMING_VELOCITY, 360.0f*DEG_TO_RAD, HOMING_CURRENT);
}
// run homing controllers
bool all_finished = false;
while(all_finished == false) {
all_finished = true;
for(int i=0; i<NUM_JOINTS; i++) {
// only update requested joints
if(((joint_mask>>i)&1) == 0) continue;
// uddate
homing_controller[i].update();
all_finished &= homing_controller[i].is_finished();
}
}
// finalize homing controllers
bool homing_successful = true;
for(int i=0; i<NUM_JOINTS; i++) {
// only check requested joints
if(((joint_mask>>i)&1) == 0) continue;
homing_controller[i].finalize();
if(homing_controller[i].is_successful()) {
joints[i]->is_homed = true;
} else {
LOG_ERROR("homing joint %i failed", i);
homing_successful = false;
}
// set joint angles
spin_lock_unsafe_blocking(shared_data.lock);
shared_data.joint_target_positions[i] = joints[i]->servo_controller->read_position();
spin_unlock_unsafe(shared_data.lock);
}
// servo updates may continue here
spin_unlock_unsafe(joints_spin_lock);
// get pose from joint angles
set_pose(pose_from_joint_angles());
// enable servo loops if all joints are initialized
enable_servo_control(true);
// check if all joints are ready
all_joints_ready = check_all_joints_ready();
return homing_successful;
}
bool Robot::calibrate_joint(int joint_idx, bool store_calibration) {
if(joint_idx<0 || joint_idx >= NUM_JOINTS)
return false;
RobotJoint* joint = joints[joint_idx];
// prevent servo loop updates from running during homing
enable_servo_control(false);
spin_lock_unsafe_blocking(joints_spin_lock);
bool calibration_ok = joint->calibrate();
if(!calibration_ok) {
spin_unlock_unsafe(joints_spin_lock);
return false;
}
// joint->servo_controller->move_to_open_loop(0.05f, 1.0);
shared_data.joint_target_positions[joint_idx] = 0; // joint->servo_controller->read_position();
if(store_calibration)
joint->store_calibration();
// servo updates may continue here
spin_unlock_unsafe(joints_spin_lock);
// recover pose from joint angles
set_pose(pose_from_joint_angles());
// enable servo loops if all joints are initialized
enable_servo_control(true);
// check if all joints are ready
all_joints_ready = check_all_joints_ready();
return true;
}
//--- G-Code Commands -------------------------------------------------------------------
bool Robot::can_process_command(const GCodeCommand& cmd) {
if(cmd.get_command() == "G0" ||
cmd.get_command() == "G4")
@ -303,15 +525,137 @@ 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() == "G1") process_motion_command(cmd, reply);
else if(cmd.get_command() == "G4") process_dwell_command(cmd, reply);
else if(cmd.get_command() == "G24") process_set_pose_command(cmd, reply);
else if(cmd.get_command() == "G28") process_home_command(cmd, reply);
else if(startswith(cmd.get_command(), "M")) process_machine_command(cmd, reply);
else reply="error: unknown command\n";
}
void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
reply = "";
// enable motors
if(cmd.get_command() == "M17") {
// read current pose from HW and set it as current pose
set_pose(pose_from_joint_angles());
// enable motors
spin_lock_unsafe_blocking(joints_spin_lock);
for(int i=0; i<NUM_JOINTS; i++) {
joints[i]->servo_controller->set_motor_enabled(true, true);
}
spin_unlock_unsafe(joints_spin_lock);
reply = "ok\n";
}
// disable motors
if(cmd.get_command() == "M18") {
spin_lock_unsafe_blocking(joints_spin_lock);
for(int i=0; i<NUM_JOINTS; i++)
joints[i]->servo_controller->set_motor_enabled(false, false);
spin_unlock_unsafe(joints_spin_lock);
reply = "ok\n";
}
// get current internal position (not using encoders to read physical position)
if(cmd.get_command() == "M50") {
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 += "\nok\n";
}
// get current internal position (not using encoders to read physical position)
if(cmd.get_command() == "M51") {
for(int i=0; i<NUM_JOINTS; i++) {
float raw_angle = joints[i]->encoder->get_last_abs_raw_angle();
float angle = joints[i]->encoder->get_last_abs_angle()*Constants::RAD2DEG;
reply += std::string("Joint ")+std::to_string(i)+": " +
std::to_string(angle) + " deg (raw="+std::to_string(raw_angle)+")\n";
}
reply += "ok\n";
}
// get planner queue size
if(cmd.get_command() == "M52") {
int s = path_planner.input_queue_size();
reply += std::string("Queue Size: ") + std::to_string(s) + "\n";
reply += "ok\n";
}
// check if all planned motions are finished executing
if(cmd.get_command() == "M53") {
bool f = path_planner.all_finished();
reply += f ? "1\n" : "0\n";
reply += "ok\n";
}
// set servo loop parameters
if(cmd.get_command() == "M55") {
process_set_servo_parameter_command(cmd, reply);
}
// calibrate joint
if(cmd.get_command() == "M56") {
process_calibrate_joint_command(cmd, reply);
}
// get info
if(cmd.get_command() == "M57") {
uint32_t servo_loop_freq = servo_loop_frequency_counter.get();
uint32_t mcontroler_freq = motion_controller_frequency_counter.get();
spin_lock_unsafe_blocking(joints_spin_lock);
for(int i=0; i<NUM_JOINTS; i++) {
float angle = joints[i]->encoder->read_abs_angle()*Constants::RAD2DEG;
reply += std::string("Joint ") + std::to_string(i)+":";
reply += std::string(" is_homed=") + std::to_string(joints[i]->is_homed);
reply += std::string(" is_calibrated=") + std::to_string(joints[i]->is_calibrated);
reply += std::string(" encoder_angle=") + std::to_string(angle) + " deg\n";
}
spin_unlock_unsafe(joints_spin_lock);
reply += std::string("Servo Loop: ") + std::to_string(servo_loop_freq/1000) + " kHz\n";
reply += std::string("Motion Controler: ") + std::to_string(mcontroler_freq) + " Hz\n";
// file list
reply += std::string("Files on flash: \n");
auto file_list = get_file_list("/", true);
for(auto& f : file_list) reply += std::string(" ")+f+"\n";
reply += "ok\n";
}
// get firmware version
if(cmd.get_command() == "M58") {
reply = std::string(FIRMWARE_VERSION)+"\n";
reply += "ok\n";
}
if(cmd.get_command() == "M59") {
int idx = (int)cmd.get_value('J', 0);
joints[idx]->servo_controller->get_enc_to_pos_lut().print_to_log();
}
// set linear and angular acceleration
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');
reply += "ok\n";
}
}
void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
Pose6DF end_pose;
if(!all_joints_ready) {
reply = "error: not all joints calibrated and homed\n";
return;
}
if(path_planner.input_queue_full()) {
reply = "error: input queue full\n";
reply = "busy\n";
return;
}
@ -350,45 +694,38 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
}
}
void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
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";
reply = "ok\n";
void Robot::process_set_pose_command(const GCodeCommand& cmd, std::string& reply) {
Pose6DF pose;
if(!all_joints_ready) {
reply = "error: not all joints calibrated and homed\n";
return;
}
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";
reply += "ok\n";
}
if(cmd.get_command() == "M52") {
int s = path_planner.input_queue_size();
reply += std::string("Queue Size: ") + std::to_string(s) + "\n";
reply += "ok\n";
}
if(cmd.get_command() == "M53") {
bool f = path_planner.all_finished();
reply += f ? "1\n" : "0\n";
reply += "ok\n";
}
if(cmd.get_command() == "M55") {
process_set_servo_parameter_command(cmd, reply);
}
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');
reply += "ok\n";
// read translation
pose.translation.x = cmd.get_value('X', current_pose.translation.x);
pose.translation.y = cmd.get_value('Y', current_pose.translation.y);
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'));
pose.rotation = QuaternionF::from_rot_vec(rot_vec);
} else {
pose.rotation = current_pose.rotation;
}
// set the current pose und update target angles for servo loops
set_pose(pose);
reply = "ok\n";
}
void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
if(!all_joints_ready) {
reply = "error: not all joints calibrated and homed\n";
return;
}
// get dwell time
float dwell_time = 1.0f;
if(cmd.has_word('S')) dwell_time = cmd.get_value('S'); // time given in seconds
@ -420,3 +757,26 @@ void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::st
reply = "ok\n";
}
void Robot::process_home_command(const GCodeCommand& cmd, std::string& reply) {
// TODO: check parameter and build joint mask
uint8_t joint_mask = 0;
for(int i=0; i<NUM_JOINTS; i++) {
if(cmd.has_word('A'+i))
joint_mask |= 1<<i;
}
if(joint_mask == 0)
joint_mask = 255;
bool ok = home(joint_mask);
reply = ok ? "ok\n" : "error\n";
}
void Robot::process_calibrate_joint_command(const GCodeCommand& cmd, std::string& reply) {
int idx = cmd.get_value('J', 0);
bool store_calibration = cmd.has_word('S');
bool ok = calibrate_joint(idx, store_calibration);
reply = ok ? "ok\n" : "error\n";
}

47
firmware/MotionControllerRP/src/robot.h
View file

@ -13,17 +13,25 @@
#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/lookup_table.h"
#include "utilities/math_constants.h"
#include "motion_control/path_planner.h"
#include "motion_control/motion_controller.h"
#include "command_parser/command_parser.h"
constexpr int ENCODER_LUT_SIZE = 256;
//*** CALSS *****************************************************************************
class Robot;
//--- PersistentData --------------------------------------------------------------------
struct PersistentRobotData {
float encoder_lut[NUM_JOINTS][ENCODER_LUT_SIZE];
};
//--- SharedData ------------------------------------------------------------------------
enum class ERobotState {
@ -35,13 +43,14 @@ enum class ERobotState {
//--- SharedData ------------------------------------------------------------------------
// shared data used to communicte between CPU cores
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];
volatile float joint_target_positions[NUM_JOINTS];
volatile float joint_target_velocities[NUM_JOINTS];
spin_lock_t* lock = nullptr;
};
@ -52,14 +61,21 @@ class RobotJoint {
RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
~RobotJoint();
void init();
void home();
void calibrate();
void init(int joint_idx);
bool calibrate();
void update(float dt, float one_over_dt);
void update_target(float p, float v);
bool load_calibration();
bool store_calibration();
private:
std::string calib_data_filename(std::string data_name) const;
public:
int joint_idx = 0;
bool is_homed = false;
bool is_calibrated = false;
float position;
float velocity;
@ -77,24 +93,32 @@ class Robot : public ICommandProcessor {
void init();
void calibrate();
void home();
bool home(uint8_t joint_mask=255);
bool calibrate_joint(int joint_idx, bool store_calibration);
void enable_servo_control(bool enable); // enables joint servo controll if homed and calibrated
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
void set_pose(const Pose6DF& pos);
Pose6DF pose_from_joint_angles();
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_set_pose_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);
void process_set_pose_command(const GCodeCommand& cmd, std::string& reply);
void process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply);
void process_home_command(const GCodeCommand& cmd, std::string& reply);
void process_calibrate_joint_command(const GCodeCommand& cmd, std::string& reply);
protected:
bool check_all_joints_ready(); // checks if all joints are homed and calibrated
static bool update_motion_controller_isr(repeating_timer_t* timer); // called from update timer
private:
@ -102,16 +126,17 @@ class Robot : public ICommandProcessor {
uint32_t path_buffering_time_us;
uint64_t path_buffering_start_time;
int motor_pole_pairs;
bool all_joints_ready;
RobotJoint* volatile joints[NUM_JOINTS];
spin_lock_t* joints_spin_lock = nullptr;
IKinemtaicModel* kinematic_model;
PathPlanner path_planner;
MotionController motion_controller;
CommandParser command_parser;
LinearAngular max_acceleration;
Pose6DF current_pose;
LinearAngular max_acceleration;
LinearAngular current_feedrate;
SharedData shared_data;

84
firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp Normal file
View file

@ -0,0 +1,84 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#include <algorithm>
#include "servo_controller.h"
#include "utilities/logging.h"
#include "utilities/math_constants.h"
#include "actuator_calibration.h"
//*** FUNCTION ***********************************************************************************/
bool measure_calibration_data(
LookupTable& encoder_raw_to_motor_pos_lut,
LookupTable& motor_pos_to_field_angle_lut,
ServoController& servo_controller,
float calibration_range,
float field_velocity,
size_t table_size)
{
LOG_INFO("Measuring motor to encoder angle lookup table...");
int sample_count = table_size*4;
// get required values
std::vector<std::pair<float, float>> motor_pos_and_field_angle;
std::vector<std::pair<float, float>> encoder_angle_and_motor_pos;
auto& motor_driver = servo_controller.get_motor_driver();
float pole_pair_count = servo_controller.get_pole_pair_count();
float start_field_angle = motor_driver.get_field_angle();
float field_angle_step = calibration_range*pole_pair_count/(sample_count-1);
auto run_measurement = [&](int sample_count, float field_angle_step) {
// Measure in increasing direction
for (size_t i = 0; i < sample_count; ++i) {
if(i>0)
motor_driver.rotate_field(field_angle_step, field_velocity, nullptr);
float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
float field_angle = motor_driver.get_field_angle();
float motor_pos = (field_angle-start_field_angle)/pole_pair_count;
// TODO: read motor_pos from precise reference encoder
encoder_angle_and_motor_pos.push_back({encoder_angle_raw, motor_pos});
motor_pos_and_field_angle.push_back({motor_pos, field_angle});
}
};
// Measure in increasing direction
LOG_DEBUG("Running foreward pass...");
run_measurement(sample_count, field_angle_step);
LOG_DEBUG("Running backward pass...");
run_measurement(sample_count, -field_angle_step);
// rotate back to start position
motor_driver.rotate_field(start_field_angle-motor_driver.get_field_angle(),
Constants::TWO_PI_F*40.0f, [&servo_controller]() {
servo_controller.get_encoder().read_abs_angle_raw();
});
// build lookup tables
bool ok = encoder_raw_to_motor_pos_lut.init_interpolating(encoder_angle_and_motor_pos, table_size, true);
encoder_raw_to_motor_pos_lut.optimize_lut(encoder_angle_and_motor_pos);
if(ok == false) {
LOG_ERROR("Creating lookup table encoder_raw_angle -> motor_pos failed.");
return false;
}
ok = motor_pos_to_field_angle_lut.init_interpolating(motor_pos_and_field_angle, table_size/2, true);
motor_pos_to_field_angle_lut.optimize_lut(motor_pos_and_field_angle);
if(ok == false) {
LOG_ERROR("Creating lookup table motor_pos -> field_angle failed.");
return false;
}
LOG_INFO("finished");
return true;
}

22
firmware/MotionControllerRP/src/servo_control/actuator_calibration.h Normal file
View file

@ -0,0 +1,22 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#pragma once
#include "servo_controller.h"
#include "utilities/lookup_table.h"
//*** FUNCTIONS *************************************************************************
bool measure_calibration_data(
LookupTable& encoder_raw_to_motor_pos_lut,
LookupTable& motor_pos_to_field_angle_lut,
ServoController& servo_controller,
float field_angle_range,
float field_velocity,
size_t size);

183
firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp
View file

@ -1,183 +0,0 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#include "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);
}
}

137
firmware/MotionControllerRP/src/servo_control/homing_controller.cpp Normal file
View file

@ -0,0 +1,137 @@
#include "homing_controller.h"
#include "utilities/math_constants.h"
#include "utilities/logging.h"
#include "pico/time.h"
HomingController::HomingController() {
retract_field_velocity = 30.0f; // rad per second
}
bool HomingController::run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current) {
start(servo_controller, motor_velocity, search_range, current);
while(is_finished() == false) {
update();
}
finalize();
return is_successful();
}
void HomingController::start(ServoController* servo_controller, float velocity, float range, float current) {
float pole_pair_count = servo_controller->get_pole_pair_count();
float field_angle_to_encoder_angle = Constants::TWO_PI_F*30.0f/3.0f*0.5f / pole_pair_count;
eval_field_angle_delta = Constants::TWO_PI_F*0.1f;
expected_encoder_delta = eval_field_angle_delta * field_angle_to_encoder_angle;
servo_ctrl = servo_controller;
field_velocity = velocity * pole_pair_count;
field_angle_search_range = range * pole_pair_count;
homing_current = current;
auto& motor_driver = servo_ctrl->get_motor_driver();
auto& encoder = servo_ctrl->get_encoder();
// perform 'soft start'
servo_ctrl->set_motor_enabled(true, false);
initial_current = servo_ctrl->get_motor_driver().get_amplitude();
servo_ctrl->get_motor_driver().set_amplitude_smooth(homing_current, 100);
search_failed = false;
// motor_driver.rotate_field(Constants::TWO_PI_F*0.5f * (field_velocity>0.0f ? -1.0f : 1.0f), 12.0f);
start_field_angle = fmodf(motor_driver.get_field_angle(), Constants::TWO_PI_F);
last_eval_encoder_angle = encoder.read_abs_angle();
last_time = 0;
state = State::Homing;
}
void HomingController::update() {
if (state != State::Homing)
return;
auto& motor_driver = servo_ctrl->get_motor_driver();
auto& encoder = servo_ctrl->get_encoder();
uint64_t time_us = time_us_64();
if(last_time == 0) last_time = time_us;
float dt = float(time_us - last_time) * 1e-6f;
last_time = time_us;
// Move motor and read encoder
field_angle_offset += field_velocity * dt;
motor_driver.set_field_angle(start_field_angle+field_angle_offset);
float encoder_angle = encoder.read_abs_angle();
if (fabs(last_eval_field_angle_offset - field_angle_offset) > eval_field_angle_delta) {
float encoder_delta = encoder_angle - last_eval_encoder_angle;
float encoder_velocity_ratio = encoder_delta / expected_encoder_delta;
// LOG_DEBUG("encoder_delta=%f/ %f", encoder_delta, expected_encoder_delta);
// LOG_DEBUG("encoder_velocity_ratio=%f", encoder_velocity_ratio);
if (fabsf(encoder_velocity_ratio) < 0.05f) {
LOG_DEBUG("End stop detected");
on_endstop_detected();
return;
}
last_eval_encoder_angle = encoder_angle;
last_eval_field_angle_offset = field_angle_offset;
}
if (fabs(field_angle_offset) > field_angle_search_range) {
LOG_INFO("End stop not detected");
search_failed = true;
finalize();
}
}
void HomingController::on_endstop_detected() {
state = State::Done;
auto& motor_driver = servo_ctrl->get_motor_driver();
auto& encoder = servo_ctrl->get_encoder();
if(search_failed) {
servo_ctrl->set_motor_enabled(false, false);
return;
}
// reset encoder period, the remainder will provide a very repeatable position reference
encoder.read_abs_angle();
servo_ctrl->get_encoder().reset_abs_angle_period();
// the motor is currently held against the end stop by the field, defining a geometric reference
home_encoder_angle = encoder.read_abs_angle();
LOG_DEBUG("home_encoder_angle=%f deg", home_encoder_angle*Constants::RAD2DEG);
if(home_encoder_angle < Constants::TWO_PI_F*0.01 || home_encoder_angle > Constants::TWO_PI_F*0.99)
LOG_WARNING("encoder angle at home position close to wrap around point !");
}
void HomingController::finalize() {
auto& motor_driver = servo_ctrl->get_motor_driver();
// back off from home position
float backoff_field_angle = Constants::TWO_PI_F*0.25f;
motor_driver.rotate_field(backoff_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f),
retract_field_velocity, nullptr);
// restore previous motor current
motor_driver.set_amplitude_smooth(initial_current, 100);
}
bool HomingController::is_finished() const {
return state == State::Done;
}
bool HomingController::is_successful() const {
return state == State::Done && !search_failed;
}
float HomingController::get_home_encoder_angle() const {
return home_encoder_angle;
}

76
firmware/MotionControllerRP/src/servo_control/homing_controller.h Normal file
View file

@ -0,0 +1,76 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#pragma once
#include "servo_controller.h"
//*** CLASS *****************************************************************************
/**
* This class implements the homing procedure for a single actuator. To allow for parallel
* homing the class is stateful and has an update() function, that can be called togeather
* with the updates of other homing controllers inside a loop.
*
* Homing procedure:
* 1. move axis in negative direction until a physical hard stop is reached
* 2. reset encoder period
* 3. back off slightly from the hard stop
*/
class HomingController {
public:
HomingController();
// Starts the homing cycle, motor_velocity can be negative and defines the homing direction.
// WARNING: Servo loop updates (including encoder reads) must be completely disabled during homing.
bool run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current);
void start(ServoController* servo_controller, float motor_velocity, float search_range, float current);
void update();
void finalize();
bool is_finished() const;
bool is_successful() const;
float get_home_encoder_angle() const;
private:
void on_endstop_detected();
float compute_eval_pos_delta(float pos, float field_angle_delta);
private:
enum class State {
Idle,
Initializing,
Homing,
Done
};
ServoController* servo_ctrl;
// Configuration params
float field_velocity = 0.0f; // defines homing direction
float field_angle_search_range = 0.0f;
float homing_current = 0.0f;
float initial_current = 0.0f;
float retract_field_velocity = 0.0f;
// State machine
State state = State::Idle;
bool search_failed = false;
// Timing
uint64_t last_time = 0;
// Offsets and tracking
float start_field_angle = 0.0f;
float field_angle_offset = 0.0f;
float last_eval_field_angle_offset = 0.0f;
float last_eval_encoder_angle = 0.0f;
float eval_field_angle_delta = 0.0f;
float expected_encoder_delta = 0.0f;
float home_encoder_angle = 0.0f;
};

56
firmware/MotionControllerRP/src/servo_control/pid.cpp
View file

@ -20,37 +20,37 @@ void PIDController::set_parameter(float kP, float kI, float kD, float output_lim
// PID controller function
float PIDController::compute(float error, float dt, float one_over_dt) {
// Proportional component
float proportional = kP * error;
float output = proportional;
// 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;
}
// 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;
}
// 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;
// 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;
integral_prev = 0.0f;
error_prev = 0.0f;
}
//--- LowpassFilter -----------------------------------------------------------
@ -67,4 +67,8 @@ float LowpassFilter::update(float value, float dt) {
float v = value_prev*alpha + (1.0f - alpha)*value;
value_prev = v;
return v;
}
}
void LowpassFilter::reset(float value) {
value_prev = value;
}

1
firmware/MotionControllerRP/src/servo_control/pid.h
View file

@ -8,6 +8,7 @@ class LowpassFilter {
void set_time_constant(float time_constant);
float update(float value, float dt);
void reset(float value);
private:
float value_prev;

242
firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
View file

@ -6,7 +6,7 @@
// --------------------------------------------------------------------------------------
#include "hardware/timer.h"
#include "Arduino.h"
#include "pico/stdlib.h"
#include "servo_controller.h"
#include "utilities/logging.h"
@ -19,11 +19,20 @@ ServoController::ServoController(
ENCODER_TYPE& encoder,
int32_t motor_pole_pair_count) :
motor_driver(motor_driver),
encoder(encoder),
motorpos_to_field_angle(motor_pole_pair_count)
encoder(encoder)
{
motor_pos = 0.0f;
pos_error = 0.0f;
ServoController::motor_pole_pair_count = motor_pole_pair_count;
ServoController::motor_update_enabled = false;
ServoController::encoder_update_enabled = true;
ServoController::motor_pos = 0.0f;
ServoController::pos_error = 0.0f;
// set default encoder lut
using namespace Constants;
float magnet_array_radius = 30.0f; // mm
float magnet_pitch = 3.0f; // mm
float g = float(encoder.get_rawcounts_per_rev())*(TWO_PI_F*magnet_array_radius/magnet_pitch)*0.5f;
build_linear_lut(encoder_raw_to_motor_pos_lut, -g, g, -TWO_PI_F, TWO_PI_F);
}
void ServoController::init(float max_motor_amplitude) {
@ -31,26 +40,41 @@ void ServoController::init(float max_motor_amplitude) {
// setup motor driver
motor_driver.begin();
motor_driver.set_amplitude(0.0f, true);
motor_driver.set_amplitude(0.0f, true); // correct amplitude will be set by 'set_motor_enabled()'
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.004f);
pos_controller.set_parameter(75.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
velocity_controller.set_parameter(0.2f, 150.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
// pos_controller.set_parameter(75.0f, 2000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
// velocity_controller.set_parameter(0.2f, 0.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
}
void ServoController::set_encoder_lut(LookupTable& enc_to_pos_lut) {
ServoController::enc_to_pos_lut = enc_to_pos_lut;
void ServoController::set_enc_to_pos_lut(LookupTable& lut) {
ServoController::encoder_raw_to_motor_pos_lut = lut;
}
void ServoController::update(float target_motor_pos, float dt, float one_over_dt) {
// get the motor position to field angle lookup table
const LookupTable& ServoController::get_enc_to_pos_lut() const {
return encoder_raw_to_motor_pos_lut;
}
void ServoController::set_pos_to_field_lut(LookupTable& lut) {
ServoController::motor_pos_to_field_angle_lut = lut;
}
// get the motor position to field angle lookup table
const LookupTable& ServoController::get_pos_to_field_lut() const {
return motor_pos_to_field_angle_lut;
}
void ServoController::update(float target_motor_pos, float dt, float one_over_dt) {
if(encoder_update_enabled == false)
return;
// read encoder
int32_t encoder_angle_raw = encoder.read_abs_angle_raw();
@ -72,7 +96,9 @@ void ServoController::update(float target_motor_pos, float dt, float one_over_dt
// 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);
if(motor_update_enabled) {
motor_driver.set_field_angle(field_angle + output);
}
// store values for next update
motor_pos_prev = motor_pos;
@ -125,93 +151,33 @@ bool ServoController::move_to(float target_motor_pos, float at_pos_eps, float se
return false;
}
void ServoController::move_to_open_loop(float target_motor_pos, float motor_angular_velocity) {
void ServoController::move_to_open_loop(float delta_motor_pos, float motor_angular_velocity) {
// Determine direction of movement at the start
const bool moving_forward = target_motor_pos > motor_pos;
const bool moving_forward = delta_motor_pos > 0.0f;
uint64_t last_time = time_us_64();
while ((moving_forward && motor_pos < target_motor_pos) ||
(!moving_forward && motor_pos > target_motor_pos))
float pos = 0.0f;
while (fabs(pos) < delta_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();
if(encoder_update_enabled)
encoder.read_abs_angle_raw();
// update motor position
motor_pos += moving_forward ? motor_angular_velocity * dt : -motor_angular_velocity * dt;
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));
float clamped_motor_pos = moving_forward ? std::min(pos, delta_motor_pos) :
std::max(pos, -delta_motor_pos);
motor_driver.set_field_angle(clamped_motor_pos*motor_pole_pair_count);
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);
motor_pos += delta_motor_pos;
}
ServoController::ENCODER_TYPE& ServoController::get_encoder() {
@ -222,69 +188,49 @@ 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);
float ServoController::get_pole_pair_count() {
return motor_pole_pair_count;
}
void ServoController::set_motor_enabled(bool enable, bool synchronize_field_angle) {
if(enable) {
// synchronize field angle to motor_pos
if(synchronize_field_angle) {
float start_field_angle = motor_pos_to_field_angle(motor_pos);
motor_driver.set_field_angle(start_field_angle);
}
motor_driver.set_amplitude_smooth(motor_current_amplitude, 100);
pos_controller.reset();
velocity_controller.reset();
velocity_lowpass.reset(0.0f);
motor_pos_prev = motor_pos;
} else {
return enc_to_pos_lut.evaluate(encoder_angle_raw);
motor_driver.set_amplitude_smooth(0.0f, 100);
}
}
// enable or disable servo loop update and encoder reads
void ServoController::set_motor_update_enabled(bool enable) {
pos_controller.reset();
velocity_controller.reset();
velocity_lowpass.reset(0.0f);
motor_pos_prev = motor_pos;
motor_update_enabled = enable;
}
void ServoController::set_encoder_update_enabled(bool enable) {
pos_controller.reset();
velocity_controller.reset();
motor_pos_prev = motor_pos;
encoder_update_enabled = enable;
}
float ServoController::encoder_angle_to_motor_pos(int32_t encoder_angle_raw) {
return encoder_raw_to_motor_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;
}
return motor_pos_to_field_angle_lut.evaluate(motor_pos);
}

70
firmware/MotionControllerRP/src/servo_control/servo_controller.h
View file

@ -9,9 +9,11 @@
#include "hardware/MT6835_encoder.h"
#include "hardware/TB6612_motor_driver.h"
#include "encoder_lut.h"
#include "utilities/lookup_table.h"
#include "pid.h"
//*** CLASS *****************************************************************************
class ServoController {
public:
// use defines instead of virtual functions for speed
@ -22,40 +24,70 @@ class ServoController {
public:
ServoController(MOTOR_DRIVER_TYPE& motor_driver, ENCODER_TYPE& encoder, int32_t motor_pole_pairs);
// initialize the servo controller hardware
void init(float max_motor_amplitude);
void set_encoder_lut(LookupTable& enc_to_pos_lut);
// set the encoder raw angle to motor position lookup table
void set_enc_to_pos_lut(LookupTable& lut);
// get the encoder raw angle to motor position lookup table
const LookupTable& get_enc_to_pos_lut() const;
// set the motor position to field angle lookup table
void set_pos_to_field_lut(LookupTable& lut);
// get the motor position to field angle lookup table
const LookupTable& get_pos_to_field_lut() const;
// Updates the servo loop.
void update(float target_motor_pos,
float dt,
float one_over_dt);
// Checks if the motor is at position (uses values from previous update() call).
bool at_position(float motor_pos_eps);
// Reads the current motor position from the encoder.
float read_position();
// Returns the motor position.
float get_position();
// Returns the current position error from the last servo loop update.
float get_position_error();
// Moves to a new motor position using closed loop control (blocking).
bool move_to(float target_motor_angle,
float at_pos_motor_angle_eps,
float settle_time_ms,
float timeout_us);
// Moves to a new motor position using open loop controll (blocking).
// Motor updates must be disabled if servo loop is running in background.
void move_to_open_loop(float target_motor_angle,
float angular_velocity);
void home(float motor_velocity, float search_range, float current=0.2f);
// Returns the encoder object.
ENCODER_TYPE& get_encoder();
MOTOR_DRIVER_TYPE& get_motor_driver();
float output;
private:
// Returns the motor driver object.
MOTOR_DRIVER_TYPE& get_motor_driver();
// Returns number of motor pole pairs.
// Can be used to approximate conversion of motor position to field angle.
float get_pole_pair_count();
// enable or disable motor
void set_motor_enabled(bool enable, bool synchronize_field_angle);
// enable or disable motor updates
void set_motor_update_enabled(bool enable);
// enable or disable encoder reads
void set_encoder_update_enabled(bool enable);
public:
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);
float motor_pos_to_field_angle_derivative(float motor_pos);
public:
LowpassFilter velocity_lowpass;
@ -65,22 +97,16 @@ class ServoController {
private:
ENCODER_TYPE& encoder;
MOTOR_DRIVER_TYPE& motor_driver;
LookupTable enc_to_pos_lut;
LookupTable encoder_raw_to_motor_pos_lut;
LookupTable motor_pos_to_field_angle_lut;
float motor_pole_pair_count = 0.0f; // number as motor pole pairs (as float to avoid repeated conversion)
float motor_current_amplitude = 0.5f; // motor current in range [0..1]
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);
float output = 0.0f; // servo loop output (field angle offset)
bool motor_update_enabled = false; // enables mootor field updates
bool encoder_update_enabled = true; // enables encoder reads
};

8
firmware/MotionControllerRP/src/utilities/logging.cpp
View file

@ -70,10 +70,10 @@ void Logger::log(ELogLevel level, const char* fmt, va_list args) {
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] ";
case ELogLevel::DEBUG: return "D)";
case ELogLevel::INFO: return "I)";
case ELogLevel::WARN: return "W)";
case ELogLevel::ERROR: return "E)";
default: return "";
}
}

464
firmware/MotionControllerRP/src/utilities/lookup_table.cpp Normal file
View file

@ -0,0 +1,464 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#include <pico/stdlib.h>
#include <LittleFS.h>
#include <algorithm>
#include "lookup_table.h"
#include "utilities/logging.h"
#include "utilities/math_constants.h"
//*** FUNCTIONS *************************************************************************
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);
}
//*** CLASS *****************************************************************************
bool LookupTable::init(int32_t table_size, float input_min, float input_max) {
lookup_table.clear();
lookup_table.resize(table_size, 0.0f);
if(input_min>=input_max) {
LOG_ERROR("LookupTable: input_max must be larger than input_min");
return false;
}
LookupTable::input_min = input_min;
LookupTable::input_max = input_max;
LookupTable::one_over_input_range = 1.0f/(input_max-input_min);
return true;
}
/*
bool LookupTable::init_approximating(std::vector<std::pair<float, float>> in_out_pairs, int table_size, float sigma) {
const int N = int(in_out_pairs.size());
if (N < 2 || table_size < 2)
return false;
// Sort by x ascending
std::sort(in_out_pairs.begin(), in_out_pairs.end(),
[](const auto& a, const auto& b) { return a.first < b.first; });
input_min = in_out_pairs.front().first;
input_max = in_out_pairs.back().first;
if (input_min >= input_max)
return false;
one_over_input_range = 1.0f / (input_max - input_min);
lookup_table.resize(table_size, 0.0f);
int start = 0;
int end = 0;
const float hw_size = (sigma*3.0f)*0.5f;
const float two_sigma_sq = 2.0f * powf(sigma, 2.0f);
for (int i = 0; i < table_size; ++i) {
// compute current evaluation position
float t = static_cast<float>(i) / (table_size - 1);
float x = input_min + t * (input_max - input_min);
// Advance window start and end index
float x_min = x - hw_size;
while (start < N - 1 && in_out_pairs[start].first < x_min) start++;
float x_max = x + hw_size;
while (end < N - 1 && in_out_pairs[end].first < x_max) end++;
// compute weighted average in window range
float sum = 0.0f;
float wcount = 0;
for (int idx = start; idx <= end; idx++) {
float dx = in_out_pairs[idx].first - x;
float w = std::exp(-(dx * dx) / two_sigma_sq);
sum += in_out_pairs[idx].second * w;
wcount += w;
}
// store weighted average value
lookup_table[i] = sum / wcount;
}
return true;
}
*/
bool LookupTable::init_interpolating(std::vector<std::pair<float, float>> in_out_pairs,
int table_size, bool sort_input)
{
if (in_out_pairs.size() < 2)
return false;
// If input is in descending order, reverse it
if(sort_input) {
std::sort(in_out_pairs.begin(), in_out_pairs.end(),
[](const auto& a, const auto& b) { return a.first < b.first; });
}
else if (in_out_pairs.front().first > in_out_pairs.back().first) {
std::reverse(in_out_pairs.begin(), in_out_pairs.end());
}
// check if values are now ascending (i.e the original input values where monotonic)
bool is_ascending = std::is_sorted(in_out_pairs.begin(), in_out_pairs.end(),
[](const auto& a, const auto& b) { return a.first < b.first; });
if(is_ascending == false) {
LOG_ERROR("LookupTable could not be initialized, input values must be monotonic");
return false;
}
// get input range
input_min = in_out_pairs.front().first;
input_max = in_out_pairs.back().first;
// check for invalid input range
if (input_min >= input_max)
return false;
one_over_input_range = 1.0f / (input_max - input_min);
// define lookup table size
lookup_table.resize(table_size, 0.0f);
int j = 0; // index in in_out_pairs
for (int i = 0; i < table_size; ++i) {
float t = static_cast<float>(i) / (table_size - 1); // normalized [0,1]
float x = input_min + t * (input_max - input_min);
// advance j until x is between in_out_pairs[j] and in_out_pairs[j + 1]
while (j + 1 < in_out_pairs.size() && x > in_out_pairs[j + 1].first) j++;
float x0 = in_out_pairs[j].first;
float y0 = in_out_pairs[j].second;
float x1 = in_out_pairs[j + 1].first;
float y1 = in_out_pairs[j + 1].second;
if (std::fabs(x1 - x0) < std::numeric_limits<float>::epsilon()) {
lookup_table[i] = y0;
} else {
float alpha = (x - x0) / (x1 - x0);
lookup_table[i] = y0 + alpha * (y1 - y0);
}
}
return true;
}
bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs) {
if (lookup_table.empty() || in_out_pairs.empty())
return false;
LOG_DEBUG("Optimizing lookup table...");
const float learning_rate = 0.02f;
const int max_iterations = 1000;
const int N = static_cast<int>(lookup_table.size());
// Gradient descent loop
std::vector<float> gradients(N, 0.0f);
float total_loss = 0.0f;
for (int iter = 0; iter < max_iterations; iter++) {
// Accumulate gradients for each pair
total_loss = 0.0f;
for (const auto &pair : in_out_pairs) {
float x = pair.first;
float y_target = pair.second;
// Compute interpolation indices and weights
int idx_a, idx_b;
float w_a, w_b;
linear_interpolate(x, idx_a, idx_b, w_a, w_b);
// Current output from LUT
float y_pred = w_a * lookup_table[idx_a] + w_b * lookup_table[idx_b];
float error = y_pred - y_target;
total_loss += error * error;
// Gradient of loss wrt y_pred = 2 * error
float grad_loss = 2.0f * error;
// Distribute gradient to LUT entries weighted by interpolation weights
gradients[idx_a] += grad_loss * w_a;
gradients[idx_b] += grad_loss * w_b;
//if(iter == max_iterations-1)
// LOG_DEBUG("error=%f", iter, error);
}
// Update LUT entries
for (int i = 0; i < N; i++) {
lookup_table[i] -= learning_rate * gradients[i];
gradients[i] = 0.0f;
}
//if(iter%100 == 0)
// LOG_DEBUG("iteration %04i: rms=%f", iter, sqrtf(total_loss));
}
LOG_DEBUG("Optimizing lookup table finished: rms=%f", total_loss);
return true;
}
void LookupTable::clear() {
lookup_table.clear();
}
// returns the size of the lookup table
uint32_t LookupTable::size() const {
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) const {
return lookup_table[idx];
}
float LookupTable::evaluate(float x) const {
if (lookup_table.size() < 2)
return 0.0f;
int idx_a, idx_b;
float weight_a, weight_b;
linear_interpolate(x, idx_a, idx_b, weight_a, weight_b);
float a = lookup_table[idx_a];
float b = lookup_table[idx_b];
return a + weight_b * (b - a);
}
void LookupTable::linear_interpolate(float x, int& idx_a, int& idx_b,
float& weight_a, float& weight_b) const
{
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) {
idx_a = idx_b = 0;
weight_a = 1.0f;
weight_b = 0.0f;
} else if (t >= 1.0f) {
idx_a = idx_b = table_size-1;
weight_a = 0.0f;
weight_b = 1.0f;
} else {
index = static_cast<size_t>(std::floor(pos));
idx_a = index;
idx_b = index + 1;
weight_b = pos - index;
weight_a = (1.0f-weight_b);
}
}
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 LookupTable::in_input_range(float x) const {
return x >= input_min && x <= input_max;
}
bool LookupTable::in_output_range(float x) const {
if(lookup_table.size() < 2) return false;
float a = lookup_table.front();
float b = lookup_table.back();
return x >= std::min(a,b) && x <= std::max(a,b);
}
void LookupTable::get_intput_range(float& input_min, float& input_max) const {
input_min = LookupTable::input_min;
input_max = LookupTable::input_max;
}
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);
}
}
//*** FUNCTION ***********************************************************************************/
void build_linear_lut(LookupTable& lut, float in_min, float in_max, float out_min, float out_max) {
lut.init(2, in_min, in_max);
lut.set_entry(0, out_min);
lut.set_entry(1, out_max);
}
bool save_lut_to_file(const LookupTable& lut, const char* filename) {
LOG_DEBUG("Saving Lookup table to file '%s'...", filename);
File file = LittleFS.open(filename, "w");
if (!file) {
LOG_ERROR("Failed to open file '%s' for writing", filename);
return false;
}
// Save metadata
uint32_t size = lut.size();
float input_min, input_max;
lut.get_intput_range(input_min, input_max);
// Write metadata (size, input_min, input_max)
if (file.write((uint8_t*)&size, sizeof(size)) != sizeof(size)) return false;
if (file.write((uint8_t*)&input_min, sizeof(input_min)) != sizeof(input_min)) return false;
if (file.write((uint8_t*)&input_max, sizeof(input_max)) != sizeof(input_max)) return false;
// Write all LUT entries
for (uint32_t i = 0; i < size; i++) {
float v = lut.get_entry(i);
if (file.write((uint8_t*)&v, sizeof(v)) != sizeof(v)) return false;
}
file.close();
// LOG_DEBUG("Saving Lookup table to file successful");
return true;
}
bool load_lut_from_file(LookupTable& lut, const char* filename) {
LOG_DEBUG("Loading Lookup table from file '%s'...", filename);
File file = LittleFS.open(filename, "r");
if (!file) {
LOG_ERROR("Failed to open file '%s' for reading", filename);
return false;
}
// Read metadata
uint32_t size = 0;
float input_min = 0.0f, input_max = 0.0f;
if (file.read((uint8_t*)&size, sizeof(size)) != sizeof(size)) return false;
if (file.read((uint8_t*)&input_min, sizeof(input_min)) != sizeof(input_min)) return false;
if (file.read((uint8_t*)&input_max, sizeof(input_max)) != sizeof(input_max)) return false;
if (!lut.init(size, input_min, input_max)) {
LOG_ERROR("Failed to create LUT from file");
file.close();
return false;
}
// Read LUT entries
for (uint32_t i = 0; i < size; i++) {
float v = 0.0f;
if (file.read((uint8_t*)&v, sizeof(v)) != sizeof(v)) {
LOG_ERROR("Reading LUT data failed");
file.close();
return false;
}
lut.set_entry(i, v);
}
file.close();
// LOG_DEBUG("Loading Lookup table from file successful");
return true;
}

36
firmware/MotionControllerRP/src/servo_control/encoder_lut.h → firmware/MotionControllerRP/src/utilities/lookup_table.h
View file

@ -11,25 +11,35 @@
#include <cstdint>
#include <cmath>
//*** CLASS *****************************************************************************
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);
bool init(int32_t table_size, float input_min, float input_max);
// initializes the lookup table from a list of input output value pairs
// approximation/interpolation will be used to sample the input range in
// equidistant steps.
bool init_interpolating(std::vector<std::pair<float, float>> in_out_pairs,
int table_size, bool sort_input);
// optimizes the lookup table using gradient descen to minimize error to provided data
bool optimize_lut(std::vector<std::pair<float, float>> in_out_pairs);
// clear the lookup table, use init to use it again
void clear();
void clear();
// returns the size of the lookup table
uint32_t size();
uint32_t size() const;
// set an entry of the lookup table
void set_entry(int32_t idx, float v);
void set_entry(int32_t idx, float v);
// set an entry of the lookup table
float get_entry(int32_t idx);
float get_entry(int32_t idx) const;
// evaluate the lookup table at a given position with linear interpolation
float evaluate(float x) const;
@ -43,9 +53,21 @@ class LookupTable {
// check if the lookup table is monotonic
bool is_monotonic() const;
// check if the given value is inside the input range
bool in_input_range(float x) const;
// check if the given value is inside the output range
bool in_output_range(float x) const;
// get input range
void get_intput_range(float& input_min, float& input_max) const;
// prints the lookup table using the logger
void print_to_log() const;
private:
void linear_interpolate(float x, int& idx_a, int& idx_b, float& weight_a, float& weight_b) const;
private:
float input_min = 0.0f;
float input_max = 0.0f;
@ -55,3 +77,7 @@ class LookupTable {
//*** FUNCTION ***********************************************************************************/
void build_linear_lut(LookupTable& lut, float in_min, float in_max, float out_min, float out_max);
bool save_lut_to_file(const LookupTable& lut, const char* filename);
bool load_lut_from_file(LookupTable& lut, const char* filename);

36
firmware/MotionControllerRP/src/utilities/utilities.cpp Normal file
View file

@ -0,0 +1,36 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#include <LittleFS.h>
#include <string>
#include "logging.h"
#include "utilities.h"
//*** FUNCTIONS *************************************************************************
std::vector<std::string> get_file_list(const char* dirname, bool include_dirs) {
std::vector<std::string> file_list;
File root = LittleFS.open(dirname, "r");
if (!root || !root.isDirectory()) {
LOG_ERROR("Failed to open directory %s", dirname);
return file_list; // return empty vector
}
File file = root.openNextFile();
while (file) {
if (!file.isDirectory()) {
file_list.emplace_back(file.name());
} else if(include_dirs) {
file_list.emplace_back(std::string("DIR ") + file.name());
}
file = root.openNextFile();
}
return file_list;
}

13
firmware/MotionControllerRP/src/utilities/utilities.h Normal file
View file

@ -0,0 +1,13 @@
// --------------------------------------------------------------------------------------
// Project: MicroManipulatorStepper
// License: MIT (see LICENSE file for full description)
// All text in here must be included in any redistribution.
// Author: M. S. (diffraction limited)
// --------------------------------------------------------------------------------------
#include <string>
#include <vector>
//*** FUNCTIONS *************************************************************************
std::vector<std::string> get_file_list(const char* dirname, bool include_dirs);

1
firmware/MotionControllerRP/src/version.h Normal file
View file

@ -0,0 +1 @@
static const char* FIRMWARE_VERSION = "v1.0.1";