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

updated firmware

Browse source
This commit is contained in:
0x23 2025-08-28 14:07:54 +02:00
parent 9891b8ed44
commit f17ca0c820
11 changed files with 273 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -46,7 +46,7 @@ float MT6835Encoder::read_abs_angle() {
return raw_angle * RAW_TO_ANGLE;
}
int32_t MT6835Encoder::read_abs_angle_raw() {
MT6835Encoder::AbsRawAngleType MT6835Encoder::read_abs_angle_raw() {
uint8_t data[6] = {0};
data[0] = MT6835_OP_ANGLE << 4;
data[1] = MT6835_REG_ANGLE1;
@ -70,6 +70,10 @@ int32_t MT6835Encoder::read_abs_angle_raw() {
return update_abs_raw_angle(raw_angle);
}
MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() {
return abs_raw_angle;
}
int32_t MT6835Encoder::get_rawcounts_per_rev() {
return MT6835_CPR;
}

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

@ -145,6 +145,7 @@ class MT6835Encoder {
float read_abs_angle(); // returns the absolute angle in radians
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
int32_t get_rawcounts_per_rev(); // returns the number of raw counts per revolution

120
firmware/MotionControllerRP/src/motion_control/path_planner.cpp
View file

@ -4,9 +4,14 @@
#include "path_planner.h"
#include "utilities/logging.h"
#include <algorithm>
PathPlanner::PathPlanner(IKinemtaicModel* kinematic_model, float time_step) {
segment_time_step = time_step;
kinematic_model = kinematic_model;
junction_deviation.linear = 0.0001; // mm
junction_deviation.angular = 0.001; // rad
}
PathPlanner::~PathPlanner() {
@ -23,9 +28,6 @@ bool PathPlanner::add_cartesian_path_segment(const CartesianPathSegment& path_se
return false;
}
// TODO: do look ahead planning of queue
new_segment->compute_motion_profile(); // for testing
return true;
}
@ -34,9 +36,15 @@ void PathPlanner::process(bool disable_interrupts_for_queue_update) {
// the segment stays in the queue until it is completed
if(segment_generator == nullptr && ct_path_segment_queue.empty() == false) {
auto* current_segment = ct_path_segment_queue.peek();
// compute motion profile ( active segment can not change anymore )
current_segment->compute_motion_profile();
// create path segment generator
segment_generator = new JointSpacePathSegmentGenerator(current_segment,
kinematic_model,
segment_time_step);
/*LOG_INFO("Starting segment: duration=%fs, (%f, %f, %f)->(%f, %f, %f) | queue size: %i",
current_segment->get_duration(),
current_segment->start_pose.translation.x,
@ -97,5 +105,111 @@ int PathPlanner::input_queue_size() {
}
void PathPlanner::run_look_ahead_planning() {
if(ct_path_segment_queue.empty())
return;
int n = ct_path_segment_queue.size();
auto* last_segment = ct_path_segment_queue.get(n - 1);
if(segment_generator != nullptr && last_segment == segment_generator->get_path_segment())
return;
// --- Reverse pass ---
// Start from the last segment, set its end velocity to zero (or target)
last_segment->end_velocity = LinearAngular{0, 0};
// Propagate backward the feasible start velocities
for (int i = n - 2; i >= 0; i--) {
CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
// dont change active segment
if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
s2->start_velocity = s1->end_velocity;
continue;
}
// Get minimum acceleration capability at junction
float acc = std::min(s1->max_acceleration.linear, s2->max_acceleration.linear);
// Compute max junction velocity feasible at junction between s1 and s2
float max_junction_velocity_linear = compute_max_junction_velocity(
s1->translation_delta_normalized,
s2->translation_delta_normalized,
acc,
junction_deviation.linear
);
// no angular junction velocity limit yet
float max_junction_velocity_angular = 1e10;
// compute max velocity delta
float v_start_max_linear = std::sqrt(std::max(0.0f, powf(s1->end_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
float v_start_max_angular = std::sqrt(std::max(0.0f, powf(s1->end_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
// compute final junction velocity
LinearAngular junction_velocity;
junction_velocity.linear = std::min(max_junction_velocity_linear, std::min(s2->target_velocity.linear, v_start_max_linear));
junction_velocity.angular = std::min(max_junction_velocity_angular, std::min(s2->target_velocity.angular, v_start_max_angular));
// set calculated velocity to both path segments
s1->end_velocity = junction_velocity;
s2->start_velocity = junction_velocity;
}
// --- Forward pass ---
for (int i = 0; i < n - 1; i++) {
CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
// dont change active segment
if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
s2->start_velocity = s1->end_velocity;
continue;
}
float v_start = s1->start_velocity.linear;
float v_end_max_linear = std::sqrt(std::max(0.0f, powf(s1->start_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
float v_end_max_angular = std::sqrt(std::max(0.0f, powf(s1->start_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
LinearAngular junction_velocity;
if(s1->end_velocity.linear > v_end_max_linear) s1->end_velocity.linear = v_end_max_linear;
if(s1->end_velocity.angular > v_end_max_angular) s1->end_velocity.angular = v_end_max_angular;
s2->start_velocity = s1->end_velocity;
}
// debug
// print_cartesian_path_segments();
}
float PathPlanner::compute_max_junction_velocity(const Vec3F& dir_in_normalized, const Vec3F& dir_out_normalized, float acceleration, float junction_deviation) {
const float EPSILON = 1e-6f;
const float COS_NEAR_STRAIGHT = 0.9999f;
const float COS_NEAR_OPPOSITE = -0.9999f;
// Compute the cosine of the angle between the directions (negative dot product)
float cos_theta = -dir_in_normalized.dot(dir_out_normalized);
// Compute sin(θ/2) using half-angle identity: sin²(θ/2) = (1 - cosθ) / 2
float sin_theta_d2 = std::sqrt(std::max(0.0f, 0.5f * (1.0f - cos_theta)));
// Compute vmax using classical junction deviation formula
float denom = std::max(1.0f - sin_theta_d2, EPSILON);
float vmax = std::sqrt(acceleration * junction_deviation * sin_theta_d2 / denom);
return vmax;
}
void PathPlanner::print_cartesian_path_segments() {
LOG_INFO("Cartesian Path Segment Info");
int n = ct_path_segment_queue.size();
for (int i = 0; i < n; i++) {
CartesianPathSegment* s = ct_path_segment_queue.get(i);
LOG_INFO(" Segment %02i: [%f, %f, %f]->[%f, %f, %f] l=%f vs=%fmm/s ve=%fmm/s", i,
s->start_pose.translation.x, s->start_pose.translation.y, s->start_pose.translation.z,
s->end_pose.translation.x, s->end_pose.translation.y, s->end_pose.translation.z,
s->travel_distance.linear,
s->start_velocity.linear, s->end_velocity.linear);
}
}

15
firmware/MotionControllerRP/src/motion_control/path_planner.h
View file

@ -13,7 +13,7 @@ class IKinemtaicModel;
class PathPlanner {
public:
static constexpr int CT_QUEUE_SIZE = 64;
static constexpr int CT_QUEUE_SIZE = 32;
static constexpr int JS_QUEUE_SIZE = 32;
public:
@ -26,6 +26,10 @@ class PathPlanner {
// adds a new cartesian space path segment to the planner queue
bool add_cartesian_path_segment(const CartesianPathSegment& path_segment);
// runs look ahead path planning. call this everytime after one or more cartesian
// path segments have been added
void run_look_ahead_planning();
// Retrieves the next joint space path segment from the queue, returns false
// if queue is empty.
bool pop_js_path_segment(JointSpacePathSegment& segment);
@ -44,7 +48,13 @@ class PathPlanner {
int input_queue_size();
private:
void run_look_ahead_planning();
float compute_max_junction_velocity(
const Vec3F& dir_in_normalized,
const Vec3F& dir_out_normalized,
float acceleration,
float junction_deviation);
void print_cartesian_path_segments();
private:
RingBuffer<CartesianPathSegment, CT_QUEUE_SIZE> ct_path_segment_queue;
@ -53,5 +63,6 @@ class PathPlanner {
IKinemtaicModel* kinematic_model;
JointSpacePathSegmentGenerator* segment_generator = nullptr;
float segment_time_step;
LinearAngular junction_deviation;
};

71
firmware/MotionControllerRP/src/motion_control/path_segment.cpp
View file

@ -21,17 +21,15 @@ MotionProfileConstAcc::MotionProfileConstAcc(
float max_velocity,
float max_acceleration)
{
MotionProfileConstAcc::acceleration = max_acceleration;
MotionProfileConstAcc::v_start = v_start;
MotionProfileConstAcc::v_end = v_end;
if (distance <= 1e-7f) {
MotionProfileConstAcc::t1 = 0.0f;
MotionProfileConstAcc::t2 = 0.0f;
MotionProfileConstAcc::t3 = 0.0f;
MotionProfileConstAcc::d1 = 0.0f;
MotionProfileConstAcc::d2 = 1.0f;
MotionProfileConstAcc::v_start = 0.0;
MotionProfileConstAcc::v_peak = 0.0f;
MotionProfileConstAcc::v_end = 0.0;
MotionProfileConstAcc::acceleration = 0.0f;
} else {
const float inv_max_acceleration = 1.0f / max_acceleration;
@ -43,40 +41,44 @@ MotionProfileConstAcc::MotionProfileConstAcc(
// Distances covered during accel/decel
float d_accel = 0.5f * (v_start + max_velocity) * t_accel;
float d_decel = 0.5f * (max_velocity + v_end) * t_decel;
float d_cruise = distance - (d_accel + d_decel);
float v_peak = 0.0;
if (d_cruise >= 0.0f) {
// Trapezoidal velocity profile
MotionProfileConstAcc::t1 = t_accel;
MotionProfileConstAcc::t2 = t1 + d_cruise / max_velocity;
MotionProfileConstAcc::t3 = t2 + t_decel;
MotionProfileConstAcc::v_peak = max_velocity;
v_peak = max_velocity;
} else {
// Triangular velocity profile: recompute peak velocity v_peak
float v_peak_sq = max_acceleration * distance + 0.5f * (v_start * v_start + v_end * v_end);
float v_peak = std::sqrt(std::max(0.0f, v_peak_sq));
v_peak = std::sqrt(std::max(0.0f, v_peak_sq));
MotionProfileConstAcc::t1 = (v_peak - v_start) * inv_max_acceleration;
MotionProfileConstAcc::t2 = t1 + 0.0f;
MotionProfileConstAcc::t3 = t2 + (v_peak - v_end) * inv_max_acceleration;
MotionProfileConstAcc::v_peak = v_peak;
}
// normalize velocity and acceleration to interpolator range (0..1)
const float inv_distance = 1.0f/distance;
max_acceleration *= inv_distance;
v_start *= inv_distance;
v_end *= inv_distance;
v_peak *= inv_distance;
// assign values
MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
MotionProfileConstAcc::v_start = v_start;
MotionProfileConstAcc::v_end = v_end;
MotionProfileConstAcc::v_peak = v_peak;
MotionProfileConstAcc::acceleration = max_acceleration;
}
// normalize velocity and acceleration to interpolator range (0..1)
const float inv_distance = 1.0f/distance;
v_start *= inv_distance;
v_end *= inv_distance;
v_peak *= inv_distance;
acceleration *= inv_distance;
// precompute some values for faster evaluation
MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
//LOG_INFO("d1=%f, d2=%f, d3=%f", d1, d2, distance);
//LOG_INFO("t1=%f, t2=%f, t3=%f", t1, t2, t3);
// LOG_INFO("d1=%f, d2=%f, d3=%f", MotionProfileConstAcc::d1, MotionProfileConstAcc::d2, distance);
// LOG_INFO("t1=%f, t2=%f, t3=%f", MotionProfileConstAcc::t1, MotionProfileConstAcc::t2, MotionProfileConstAcc::t3);
// LOG_INFO("vs=%f, vp=%f, ve=%f", MotionProfileConstAcc::v_start, MotionProfileConstAcc::v_peak, MotionProfileConstAcc::v_end);
}
float MotionProfileConstAcc::evaluate(float time) const {
@ -119,8 +121,19 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
CartesianPathSegment::max_acceleration = max_acceleration;
travel_distance.linear = (end_pose.translation - start_pose.translation).length();
Vec3F translation_delta = end_pose.translation - start_pose.translation;
travel_distance.linear = (translation_delta).length();
travel_distance.angular = (start_pose.rotation.normalized_inverse() * end_pose.rotation).angle();
translation_delta_normalized = translation_delta.normalized();
/*
QuaternionF rotation_delta = (end_pose.rotation * start_pose.rotation.normalized_inverse());
Vec3F axis;
float angle;
rotation_delta.to_axis_angle(axis, angle);
rotation_delta_axis = axis; */
}
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
@ -144,9 +157,9 @@ void CartesianPathSegment::compute_motion_profile() {
if(dwell_time > 0.0f) {
motion_profile = MotionProfileConstAcc(dwell_time);
} else {
MotionProfileConstAcc linear_profile(travel_distance.linear,start_velocity.linear,
end_velocity.linear, target_velocity.linear,
max_acceleration.linear);
MotionProfileConstAcc linear_profile(travel_distance.linear, start_velocity.linear,
end_velocity.linear, target_velocity.linear,
max_acceleration.linear);
MotionProfileConstAcc angular_profile(travel_distance.angular, start_velocity.angular,
end_velocity.angular, target_velocity.angular,
@ -164,6 +177,7 @@ void CartesianPathSegment::compute_motion_profile() {
void CartesianPathSegment::evaluate(float time, Pose6DF& pose) const {
// evaluate motion profile
float t = motion_profile.evaluate(time);
// LOG_INFO(">t_x [mm]: %f", t);
// interpolate pose
pose = Pose6DF::lerp(start_pose, end_pose, t);
@ -265,6 +279,7 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
// evaluate path to get new end position
Pose6DF seg_end_pose;
path_segment->evaluate(current_time, seg_end_pose);
// LOG_INFO(">pos_x [mm]: %f", seg_end_pose.translation.x);
// evaluate inverse kinematic model here
float next_joint_pos[NUM_JOINTS];
@ -279,4 +294,8 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
current_joint_pos[i] = next_joint_pos[i];
return end_reached;
}
}
const CartesianPathSegment* JointSpacePathSegmentGenerator::get_path_segment() const {
return path_segment;
}

10
firmware/MotionControllerRP/src/motion_control/path_segment.h
View file

@ -64,7 +64,6 @@ class CartesianPathSegment {
void evaluate(float time, Pose6DF& pose) const;
float get_duration() const;
void compute_motion_profile();
public:
@ -76,6 +75,10 @@ class CartesianPathSegment {
LinearAngular end_velocity;
LinearAngular max_acceleration;
LinearAngular max_velocity_delta;
Vec3F translation_delta_normalized;
//Vec3F rotation_delta_axis;
LinearAngular travel_distance;
MotionProfileConstAcc motion_profile;
@ -121,8 +124,9 @@ class JointSpacePathSegmentGenerator {
float time_step
);
void reset();
bool generate_next(JointSpacePathSegment& js_path_segment);
void reset();
bool generate_next(JointSpacePathSegment& js_path_segment);
const CartesianPathSegment* get_path_segment() const;
private:
float delta_time; // time step size

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

@ -88,8 +88,9 @@ Robot::Robot(float path_segment_time_step) :
command_parser.set_command_processor(this);
current_feedrate = LinearAngular(10.0f, 1.0f);
max_acceleration = LinearAngular(500.0f, 50.0f);
path_buffering_time_us = 100*1000;
path_buffering_time_us = 50*1e3;
state = ERobotState::IDLE;
}
@ -183,6 +184,13 @@ void Robot::update_command_parser() {
// update command parse which will queue command to the path planner
command_parser.update();
// TESTING:
//sleep_ms(10);
//float pos_error = joints[1]->servo_controller->get_position_error();
//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);
}
/**
@ -194,15 +202,14 @@ void Robot::update_path_planner() {
// check if buffering starts
uint64_t time = time_us_64();
if(state == ERobotState::IDLE && path_planner.input_queue_size() > 0) {
state = ERobotState::BUFFERING_PAH;
state = ERobotState::BUFFERING_PATH;
path_buffering_start_time = time;
}
// check if execution starts
uint64_t buffering_time = time-path_buffering_start_time;
if(state == ERobotState::BUFFERING_PAH && buffering_time > path_buffering_time_us) {
if(state == ERobotState::BUFFERING_PATH && buffering_time > path_buffering_time_us) {
state = ERobotState::EXECUTING_PATH;
path_buffering_start_time = time_us_64();
}
// execute path
@ -287,6 +294,7 @@ void Robot::send_reply(const char* str) {
void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
if(cmd.get_command() == "G0") process_motion_command(cmd, reply);
else if(cmd.get_command() == "G1") process_motion_command(cmd, reply);
else if(cmd.get_command() == "G4") process_dwell_command(cmd, reply);
else if(startswith(cmd.get_command(), "M")) process_machine_command(cmd, reply);
else reply="error: unknown command\n";
@ -295,9 +303,17 @@ void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
Pose6DF end_pose;
if(path_planner.input_queue_full()) {
reply = "error: input queue full\n";
return;
}
// read feed rate
float feed_linear = cmd.get_value('F', 10.0f);
float feed_angular = cmd.get_value('R', 1.0f);
current_feedrate.linear = cmd.get_value('F', current_feedrate.linear);
current_feedrate.angular = cmd.get_value('R', current_feedrate.angular);
if(cmd.has_word('I'))
state = ERobotState::EXECUTING_PATH;
// read translation
end_pose.translation.x = cmd.get_value('X', current_pose.translation.x);
@ -314,33 +330,54 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
// create path segment
CartesianPathSegment path_segment(current_pose, end_pose,
LinearAngular(feed_linear, feed_angular),
current_feedrate,
max_acceleration);
path_planner.add_cartesian_path_segment(path_segment);
current_pose = end_pose;
reply = "ok\n";
bool ok = path_planner.add_cartesian_path_segment(path_segment);
if(ok) {
path_planner.run_look_ahead_planning();
current_pose = end_pose;
reply = "ok\n";
} else {
reply = "error\n";
}
}
void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
reply = "";
if(cmd.get_command() == "M50") {
reply = "Current Position: ";
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";
}
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";
}
}
@ -353,8 +390,26 @@ void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
// create path segment
CartesianPathSegment path_segment(current_pose, dwell_time);
path_planner.add_cartesian_path_segment(path_segment);
path_planner.run_look_ahead_planning();
reply = "ok\n";
}
void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply) {
// example: M55 A150 B50000 C0.2 D100 E F0.0025
bool has_all = cmd.has_word('A') && cmd.has_word('B') && cmd.has_word('C') &&
cmd.has_word('D') && cmd.has_word('F');
if(has_all == false)
reply = "error: not all parameters given (A,B,C,D,F expected)\n";
for(int i=0; i<NUM_JOINTS; i++) {
joints[i]->servo_controller->velocity_lowpass.set_time_constant(cmd.get_value('F'));
joints[i]->servo_controller->pos_controller.set_parameter(cmd.get_value('A'), cmd.get_value('B'), 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
joints[i]->servo_controller->velocity_controller.set_parameter(cmd.get_value('C'), cmd.get_value('D'), 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
}
reply = "ok\n";
}

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

@ -21,7 +21,7 @@ class Robot;
enum class ERobotState {
IDLE = 0,
BUFFERING_PAH = 1,
BUFFERING_PATH = 1,
EXECUTING_PATH = 2,
ERROR = 3
};
@ -84,6 +84,8 @@ class Robot : public ICommandProcessor {
void process_motion_command(const GCodeCommand& cmd, std::string& reply);
void process_dwell_command(const GCodeCommand& cmd, std::string& reply);
void process_machine_command(const GCodeCommand& cmd, std::string& reply);
void process_set_pose_command(const GCodeCommand& cmd, std::string& reply);
void process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply);
protected:
static bool update_motion_controller_isr(repeating_timer_t* timer); // called from update timer
@ -102,6 +104,7 @@ class Robot : public ICommandProcessor {
LinearAngular max_acceleration;
Pose6DF current_pose;
LinearAngular current_feedrate;
SharedData shared_data;

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

@ -34,9 +34,9 @@ void ServoController::init(float max_motor_amplitude) {
sleep_ms(1);
}
velocity_lowpass.set_time_constant(0.0025f);
pos_controller.set_parameter(150.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
velocity_controller.set_parameter(0.2f, 100.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
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);
}
void ServoController::set_encoder_lut(LookupTable& enc_to_pos_lut) {

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

@ -50,15 +50,16 @@ class ServoController {
float motor_pos_to_field_angle(float motor_pos);
float motor_velocity_to_field_velocity(float v);
public:
LowpassFilter velocity_lowpass;
PIDController pos_controller;
PIDController velocity_controller;
private:
ENCODER_TYPE& encoder;
MOTOR_DRIVER_TYPE& motor_driver;
LookupTable enc_to_pos_lut;
LowpassFilter velocity_lowpass;
PIDController pos_controller;
PIDController velocity_controller;
float motor_pos = 0; // current motor position
float motor_pos_prev = 0; // previous motor position
float pos_error = 0; // current position error as computed by upate()

5
firmware/MotionControllerRP/src/utilities/ringbuffer.h
View file

@ -33,6 +33,11 @@ class RingBuffer {
return &(buffer[tail]);
}
T* get(int i) {
if (count == 0) return nullptr;
return &(buffer[(tail + i) % N]);
}
bool empty() const { return count == 0; }
bool full() const { return count == N; }
int size() const { return count; }