Version v1.0.1:
* 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
parent
2cf353e7fc
commit
d9888ef369
27 changed files with 1723 additions and 784 deletions
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,57 +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)
|
||||
// --------------------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
class LookupTable {
|
||||
public:
|
||||
LookupTable() {}
|
||||
|
||||
// initializes the lookup table to a given size and input range
|
||||
void init(int32_t size, float input_min, float input_max);
|
||||
|
||||
// clear the lookup table, use init to use it again
|
||||
void clear();
|
||||
|
||||
// returns the size of the lookup table
|
||||
uint32_t size();
|
||||
|
||||
// set an entry of the lookup table
|
||||
void set_entry(int32_t idx, float v);
|
||||
|
||||
// set an entry of the lookup table
|
||||
float get_entry(int32_t idx);
|
||||
|
||||
// evaluate the lookup table at a given position with linear interpolation
|
||||
float evaluate(float x) const;
|
||||
|
||||
// evaluate the inverse of the lookup table function (very slow), the LUT must be monotonic
|
||||
float evaluate_inverse(float y) const;
|
||||
|
||||
// inverts the lookup table so it represents the funcion x = fi(y) given y = f(x)
|
||||
bool invert(int new_size);
|
||||
|
||||
// check if the lookup table is monotonic
|
||||
bool is_monotonic() const;
|
||||
|
||||
// prints the lookup table using the logger
|
||||
void print_to_log() const;
|
||||
|
||||
private:
|
||||
float input_min = 0.0f;
|
||||
float input_max = 0.0f;
|
||||
float one_over_input_range = 1.0f;
|
||||
std::vector<float> lookup_table;
|
||||
};
|
||||
|
||||
//*** FUNCTION ***********************************************************************************/
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
};
|
||||
Loading…
Add table
Add a link
Reference in a new issue