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
|
|
@ -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";
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue