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

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;