149 lines
4.8 KiB
C++
149 lines
4.8 KiB
C++
// --------------------------------------------------------------------------------------
|
|
// 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 "utilities/logging.h"
|
|
#include "utilities/frequency_counter.h"
|
|
#include "hardware/MT6701_encoder.h"
|
|
#include "hardware/MT6835_encoder.h"
|
|
#include "hardware/TB6612_motor_driver.h"
|
|
#include "servo_control/servo_controller.h"
|
|
#include "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 {
|
|
IDLE = 0,
|
|
BUFFERING_PATH = 1,
|
|
EXECUTING_PATH = 2,
|
|
ERROR = 3
|
|
};
|
|
|
|
//--- 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_target_positions[NUM_JOINTS];
|
|
volatile float joint_target_velocities[NUM_JOINTS];
|
|
spin_lock_t* lock = nullptr;
|
|
};
|
|
|
|
//--- RobotJoint ------------------------------------------------------------------------
|
|
|
|
class RobotJoint {
|
|
public:
|
|
RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
|
|
~RobotJoint();
|
|
|
|
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;
|
|
|
|
MT6835Encoder* encoder;
|
|
TB6612MotorDriver* motor_driver;
|
|
ServoController* servo_controller;
|
|
};
|
|
|
|
//--- Robot -----------------------------------------------------------------------------
|
|
|
|
class Robot : public ICommandProcessor {
|
|
public:
|
|
Robot(float path_segment_time_step);
|
|
~Robot();
|
|
|
|
void init();
|
|
void calibrate();
|
|
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_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:
|
|
ERobotState state;
|
|
uint32_t path_buffering_time_us;
|
|
uint64_t path_buffering_start_time;
|
|
|
|
bool all_joints_ready;
|
|
RobotJoint* volatile joints[NUM_JOINTS];
|
|
spin_lock_t* joints_spin_lock = nullptr;
|
|
|
|
IKinematicModel* kinematic_model;
|
|
PathPlanner path_planner;
|
|
MotionController motion_controller;
|
|
CommandParser command_parser;
|
|
|
|
Pose6DF current_pose;
|
|
LinearAngular max_acceleration;
|
|
LinearAngular current_feedrate;
|
|
|
|
SharedData shared_data;
|
|
|
|
struct repeating_timer motion_controller_update_timer;
|
|
uint64_t last_mc_update_time;
|
|
|
|
FrequencyCounter servo_loop_frequency_counter;
|
|
FrequencyCounter motion_controller_frequency_counter;
|
|
};
|