WIP calibration tool.

Author: aentinger

examples/Tools/Calibration/Calibration.ino

@@ -0,0 +1,114 @@
+/* This example is used for calibrating the zero angles
+ * for each motor of the robot arm prior to assembly of
+ * the arm.
+ */
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
+#include <Braccio++.h>
+
+/**************************************************************************************
+ * TYPEDEF
+ **************************************************************************************/
+
+class InitialServoPosition
+{
+public:
+  InitialServoPosition(int const motor_id, float const motor_angle_deg)
+  : _motor_id{motor_id}
+  , _motor_angle_deg{motor_angle_deg}
+  { }
+
+  inline int id() const { return _motor_id; }
+  inline float angle() const { return _motor_angle_deg; }
+
+private:
+  int const _motor_id;
+  float const _motor_angle_deg;
+};
+
+/**************************************************************************************
+ * CONSTANT
+ **************************************************************************************/
+
+static std::array<InitialServoPosition, 6> const INITIAL_SERVO_POSITION =
+{
+  InitialServoPosition{1, 0.0},
+  InitialServoPosition{2, 0.0},
+  InitialServoPosition{3, 0.0},
+  InitialServoPosition{4, 0.0},
+  InitialServoPosition{5, 0.0},
+  InitialServoPosition{6, 0.0},
+};
+
+/**************************************************************************************
+ * FUNCTIONS
+ **************************************************************************************/
+
+void calibrate(int const id, float const target_angle)
+{
+  Serial.print("Connecting .... ");
+  for (; !Braccio.get(id).connected(); delay(10)) { }
+  Serial.println("OK.");
+  delay(500);
+
+  Serial.print("Disengaging ... ");
+  Braccio.get(id).disengage();
+  Serial.println("OK.");
+  delay(500);
+
+  Serial.print("Engaging ...... ");
+  Braccio.get(id).engage();
+  Serial.println("OK.");
+  delay(500);
+
+  /* Drive to the position for assembling the servo horn. */
+  for (float current_angle = Braccio.get(id).position();
+       fabs(Braccio.get(id).position() - target_angle) > 1.0;)
+  {
+    Braccio.get(id).move().to(target_angle).in(200ms);
+    delay(250);
+  
+    char msg[64] = {0};
+    snprintf(msg, sizeof(msg), "Angle (Target | Current) = %.2f | %.2f", target_angle, Braccio.get(id).position());
+    Serial.println(msg);
+  }
+}
+
+/**************************************************************************************
+ * SETUP/LOOP
+ **************************************************************************************/
+
+void setup()
+{
+  Serial.begin(115200);
+  while(!Serial) { }
+
+  Braccio.begin();
+  delay(100);
+  Braccio.disengage();
+  delay(100);
+
+  calibrate(1, 0.0);
+
+/*
+  for (auto & servo : INITIAL_SERVO_POSITION)
+  {
+    Serial.print("Servo ");
+    Serial.print(servo.id());
+    Serial.print(": Angle = ");
+    Serial.print(servo.angle());
+    Serial.print(" °");
+    Serial.println();
+
+    calibrate(servo.id(), servo.angle());
+  }
+*/
+}
+
+void loop()
+{
+
+}