added measurement print option to calibration

firmware/MotionControllerRP/src/hw_config.h

@@ -13,7 +13,7 @@
 #define HOMING_FINISH_POS 0.5f        // in rad
 
 
-#define SINGLE_AXIS_BOARD
+// #define SINGLE_AXIS_BOARD
 #ifndef SINGLE_AXIS_BOARD
   // Pins for 3Axis Board
   #define PIN_BUILTIN_LED 23

firmware/MotionControllerRP/src/main.cpp

@@ -68,7 +68,6 @@ void overclock() {
 }
 
 void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
-  return;
   strip.neoPixelSetValue(0, r, g, b, false);
   delayMicroseconds(2000);
   strip.neoPixelShow();

firmware/MotionControllerRP/src/robot.cpp

@@ -58,7 +58,7 @@ void RobotJoint::init(int joint_idx) {
   servo_controller->set_motor_enabled(false, false);
 }
 
-bool RobotJoint::calibrate() {
+bool RobotJoint::calibrate(bool print_measurements) {
   LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
 
   HomingController homing_controller;
@@ -77,7 +77,8 @@ bool RobotJoint::calibrate() {
                                      *servo_controller, 
                                      CALIBRATION_RANGE*DEG_TO_RAD,
                                      CALIBRATION_FIELD_VELOCITY, 
-                                     256);
+                                     256,
+                                     print_measurements);
   if(!ok) {
     LOG_ERROR("Joint-%i: calibrating failed", joint_idx);
     return false;
@@ -468,7 +469,7 @@ bool Robot::home(uint8_t joint_mask) {
   return homing_successful;
 }
 
-bool Robot::calibrate_joint(int joint_idx, bool store_calibration) {
+bool Robot::calibrate_joint(int joint_idx, bool store_calibration, bool print_measurements) {
   if(joint_idx<0 || joint_idx >= NUM_JOINTS)
     return false;
 
@@ -478,7 +479,7 @@ bool Robot::calibrate_joint(int joint_idx, bool store_calibration) {
   enable_servo_control(false);
   spin_lock_unsafe_blocking(joints_spin_lock);
 
-  bool calibration_ok = joint->calibrate();
+  bool calibration_ok = joint->calibrate(print_measurements);
   if(!calibration_ok) {
     spin_unlock_unsafe(joints_spin_lock);
     return false;
@@ -634,6 +635,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     reply += "ok\n";
   }
 
+  // print lookup table
   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();
@@ -776,7 +778,8 @@ void Robot::process_home_command(const GCodeCommand& cmd, std::string& reply) {
 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 print_measurements = cmd.has_word('P');
 
-  bool ok = calibrate_joint(idx, store_calibration);
+  bool ok = calibrate_joint(idx, store_calibration, print_measurements);
   reply = ok ? "ok\n" : "error\n";
 }

firmware/MotionControllerRP/src/robot.h

@@ -62,7 +62,7 @@ class RobotJoint {
     ~RobotJoint();
 
     void init(int joint_idx);
-    bool calibrate();
+    bool calibrate(bool print_measurements);
     void update(float dt, float one_over_dt);
     void update_target(float p, float v);
     bool load_calibration();
@@ -94,7 +94,7 @@ class Robot : public ICommandProcessor {
     void init();
     void calibrate();
     bool home(uint8_t joint_mask=255);
-    bool calibrate_joint(int joint_idx, bool store_calibration);
+    bool calibrate_joint(int joint_idx, bool store_calibration, bool print_measurements);
     void enable_servo_control(bool enable);  // enables joint servo controll if homed and calibrated
 
     void update_command_parser();            // called from main loop

firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp

@@ -22,7 +22,8 @@ bool measure_calibration_data(
             ServoController& servo_controller,
             float calibration_range,
             float field_velocity,
-            size_t table_size)
+            size_t table_size,
+            bool print_measurements)
 {
   LOG_INFO("Measuring motor to encoder angle lookup table...");
   int sample_count = table_size*4;
@@ -47,6 +48,9 @@ bool measure_calibration_data(
       float motor_pos = (field_angle-start_field_angle)/pole_pair_count;
       // TODO: read motor_pos from precise reference encoder
 
+      if(print_measurements)
+        LOG_INFO("%f, %15.10f, %15.10f", encoder_angle_raw, field_angle, motor_pos);
+
       encoder_angle_and_motor_pos.push_back({encoder_angle_raw, motor_pos});
       motor_pos_and_field_angle.push_back({motor_pos, field_angle});
     }

firmware/MotionControllerRP/src/servo_control/actuator_calibration.h

@@ -18,5 +18,6 @@ bool measure_calibration_data(
   ServoController& servo_controller,
   float field_angle_range,
   float field_velocity,
-  size_t size);
+  size_t size,
+  bool print_measurements);