added files

firmware/MotionControllerRP/src/robot.cpp

@@ -0,0 +1,360 @@
+#include "robot.h"
+#include "hw_config.h"
+#include "utilities/logging.h"
+#include "kinemtaic_models/kinematic_model_delta3d.h"
+
+//*** FUNCTION **************************************************************************
+
+bool startswith(const std::string& str, const std::string& prefix) {
+    return str.size() >= prefix.size() &&
+           std::equal(prefix.begin(), prefix.end(), str.begin());
+}
+
+//*** CLASS *****************************************************************************
+
+//--- RobotAxis -------------------------------------------------------------------------
+
+RobotJoint::RobotJoint(MT6835Encoder* encoder, 
+                     TB6612MotorDriver* motor_driver,
+                     int pole_pairs) 
+{
+  RobotJoint::encoder = encoder;
+  RobotJoint::motor_driver = motor_driver;
+  servo_controller = new ServoController(*motor_driver, *encoder, pole_pairs);
+  position = 0.0f;
+  velocity = 0.0f;
+}
+
+RobotJoint::~RobotJoint() {
+  delete servo_controller;
+  delete motor_driver;
+  delete encoder;
+  servo_controller = nullptr;
+  motor_driver = nullptr;
+  encoder = nullptr;
+}
+
+void RobotJoint::init() {
+  encoder->init(0x5, 0x4);
+  servo_controller->init(0.5);
+}
+
+void RobotJoint::home() {
+  servo_controller->home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
+  position = servo_controller->get_position();
+  velocity = 0.0f;
+}
+
+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);
+  }
+  LOG_DEBUG(">finished");
+  // lut.print_to_log();
+  delay(200);
+
+  servo_controller->set_encoder_lut(lut);
+}
+
+void RobotJoint::update(float dt, float one_over_dt) {
+  servo_controller->update(position, dt, one_over_dt);
+}
+
+void RobotJoint::update_target(float p, float v) {
+  position = p;
+  velocity = v;
+}
+
+//--- 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)
+{
+  kinematic_model = new KinematicModel_Delta3D();
+  path_planner.set_kinematic_model(kinematic_model);
+
+  for(int i=0; i<3; i++)
+    joints[i] = nullptr;
+
+  command_parser.set_command_processor(this);
+
+  max_acceleration = LinearAngular(500.0f, 50.0f);
+  path_buffering_time_us = 100*1000;
+
+  state = ERobotState::IDLE;
+}
+
+Robot::~Robot() {
+  if(kinematic_model != nullptr)
+    delete kinematic_model;
+
+  for(int i=0; i<3; i++) {
+    if(joints[i] != nullptr)
+      delete joints[i];
+    joints[i] = nullptr;
+  }
+}
+
+void Robot::init() {
+  MT6835Encoder::setup_spi(spi0, PIN_ENCODER_SCK, PIN_ENCODER_MOSI, PIN_ENCODER_MISO, 8000000);
+
+  // axis 1
+  {
+    auto* encoder = new MT6835Encoder(spi0, PIN_ENCODER1_CS);
+    auto* motor_driver = new TB6612MotorDriver(
+      PIN_MOTOR_EN, PIN_M1_PWM_A_POS, PIN_M1_PWM_A_NEG, PIN_MOTOR_PWMAB,
+      PIN_MOTOR_EN, PIN_M1_PWM_B_POS, PIN_M1_PWM_B_NEG, PIN_MOTOR_PWMAB
+    );
+    joints[0] = new RobotJoint(encoder, motor_driver, 400/4);
+  }
+
+  // axis 2
+  {
+    auto* encoder = new MT6835Encoder(spi0, PIN_ENCODER2_CS);
+    auto* motor_driver = new TB6612MotorDriver(
+      PIN_MOTOR_EN, PIN_M2_PWM_A_POS, PIN_M2_PWM_A_NEG, PIN_MOTOR_PWMAB,
+      PIN_MOTOR_EN, PIN_M2_PWM_B_POS, PIN_M2_PWM_B_NEG, PIN_MOTOR_PWMAB
+    );
+    joints[1] = new RobotJoint(encoder, motor_driver, 400/4);
+  }
+
+  // axis 3
+  {
+    auto* encoder = new MT6835Encoder(spi0, PIN_ENCODER3_CS);
+    auto* motor_driver = new TB6612MotorDriver(
+      PIN_MOTOR_EN, PIN_M3_PWM_A_POS, PIN_M3_PWM_A_NEG, PIN_MOTOR_PWMAB,
+      PIN_MOTOR_EN, PIN_M3_PWM_B_POS, PIN_M3_PWM_B_NEG, PIN_MOTOR_PWMAB
+    );
+    joints[2] = new RobotJoint(encoder, motor_driver, 400/4);
+  }
+
+  // initialize axes
+  for(int i=0; i<3; i++) {
+    joints[i]->init();
+  }
+
+  // setup timer for updating the motion controller (which evaluates joint space path
+  // segments and produces the current target position for the servo loops)
+  float motion_controller_update_time_us = 500;
+  add_repeating_timer_us(-motion_controller_update_time_us, 
+                         Robot::update_motion_controller_isr, 
+                         (void*)this, 
+                         &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
+  if (Serial.available()) {
+    char c = Serial.read();
+    command_parser.add_input_character(c);
+    // Serial.write(c);
+  }
+
+  // update command parse which will queue command to the path planner
+  command_parser.update();
+}
+
+/**
+ * Updates the path planner, that chops up kartesian path segments into joint space
+ * path segments using the inverse kinematic model. It then enqueues these joint space path
+ * segments for the motion controller.
+ */
+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;
+    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) {
+    state = ERobotState::EXECUTING_PATH;
+    path_buffering_start_time = time_us_64();
+  }
+
+  // execute path
+  if(state == ERobotState::EXECUTING_PATH) {
+    // update planner and generate joint space path segments
+    path_planner.process(true);
+
+    if(path_planner.all_finished())
+      state = ERobotState::IDLE;
+  }
+}
+
+/**
+ * Updates the motion controller with a timer interrupt in regular intervals.
+ * The function evaluates joint space path segments and produces the current 
+ * target position for the servo loops.
+ */
+bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
+  float joint_positions[NUM_JOINTS];
+  float joint_velocities[NUM_JOINTS];
+
+  // get robot pointer
+  Robot* robot = (Robot*)timer->user_data;
+
+  // get time and delta time
+  uint64_t time_us = time_us_64();
+  float dt = float(time_us - robot->last_mc_update_time)*1e-6f;
+  robot->last_mc_update_time = time_us;
+
+  // get current joint position/velocity
+  bool update_ok = robot->motion_controller.update(dt, joint_positions, joint_velocities);
+
+  // 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];
+    }
+    spin_unlock_unsafe(robot->shared_data.lock);
+  }
+
+  // update frequency counter
+  robot->motion_controller_frequency_counter.update(dt);
+
+  return true; // keep repeating
+}
+
+/**
+ * update servo loops, this is called from a second cpu core
+ */
+void Robot::update_servo_controllers(float dt) {
+  float one_over_dt = 1.0f/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]);
+  spin_unlock_unsafe(shared_data.lock);
+
+  // update servo loop for each axis
+  for(int i=0; i<3; i++) {
+    joints[i]->update(dt, one_over_dt);
+  }
+
+  // update frequency counter
+  servo_loop_frequency_counter.update(dt);
+}
+
+bool Robot::can_process_command(const GCodeCommand& cmd) {
+  if(cmd.get_command() == "G0" || 
+     cmd.get_command() == "G4")
+  {
+    return path_planner.input_queue_full() == false;
+  }
+
+  return true;
+}
+
+void Robot::send_reply(const char* str) {
+  Serial.write(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() == "G4") process_dwell_command(cmd, reply);
+  else if(startswith(cmd.get_command(), "M")) process_machine_command(cmd, reply);
+  else reply="error: unknown command\n";
+}
+
+void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
+  Pose6DF end_pose;
+
+  // read feed rate
+  float feed_linear = cmd.get_value('F', 10.0f);
+  float feed_angular = cmd.get_value('R', 1.0f);
+
+  // read translation
+  end_pose.translation.x = cmd.get_value('X', current_pose.translation.x);
+  end_pose.translation.y = cmd.get_value('Y', current_pose.translation.y);
+  end_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'));
+    end_pose.rotation = QuaternionF::from_rot_vec(rot_vec);
+  } else {
+    end_pose.rotation = current_pose.rotation;
+  }
+
+  // create path segment
+  CartesianPathSegment path_segment(current_pose, end_pose, 
+                                    LinearAngular(feed_linear, feed_angular), 
+                                    max_acceleration);
+
+  path_planner.add_cartesian_path_segment(path_segment);
+  current_pose = end_pose;
+
+  reply = "ok\n";
+}
+
+void Robot::process_machine_command(const GCodeCommand& cmd, std::string& 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";
+  }
+  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";
+  }
+  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');
+  }
+}
+
+void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
+  // get dwell time
+  float dwell_time = 1.0f;
+  if(cmd.has_word('S')) dwell_time = cmd.get_value('S');          // time given in seconds
+  if(cmd.has_word('P')) dwell_time = cmd.get_value('P')*0.001f;   // time given in milliseconds
+
+  // create path segment
+  CartesianPathSegment path_segment(current_pose, dwell_time);
+  path_planner.add_cartesian_path_segment(path_segment);
+
+  reply = "ok\n";
+}
+
+