[WIP] Tool for aligning all motor joints.

Author: aentinger

examples/Tools/Calibration/Calibration.ino

@@ -0,0 +1,89 @@
+/* This example is used for calibrating the zero angles
+ * for each motor of the robot arm.
+ */
+
+#include <Braccio++.h>
+
+void calibrate(int const motor_id)
+{
+  while (!Braccio.get(motor_id).connected()) { }
+  Braccio.get(motor_id).disengage();
+  delay(500);
+  Braccio.get(motor_id).engage();
+  delay(500);
+
+  float target_position_deg = Braccio.get(motor_id).position();
+  /* This is necessary because right now the library does not always report the right position from the very start ... */
+  for(; target_position_deg > 360.0; target_position_deg = Braccio.get(motor_id).position()); { delay(100); }
+
+  while(!Braccio.isButtonPressed_ENTER())
+  {
+    Serial.print("[");
+    Serial.print(motor_id);
+    Serial.print("] ");
+    Serial.print("Position (Current | Target) = ");
+    Serial.print(Braccio.get(motor_id).position());
+    Serial.print(" | ");
+    Serial.println(target_position_deg);
+
+    if(Braccio.isJoystickPressed_LEFT())
+      target_position_deg -= 1.0;
+    if(Braccio.isJoystickPressed_RIGHT())
+      target_position_deg += 1.0;
+
+    target_position_deg = max(target_position_deg, 0.0);
+    target_position_deg = min(target_position_deg, 360.0);
+
+    delay(50);
+
+    /* Only call move().to() every 500 ms. Otherwise you flood the bus with messages and there you have the salad ... */
+    static auto now = millis();
+    if ((millis() - now) > 500)
+    {
+      Braccio.get(motor_id).move().to(target_position_deg).in(5s);
+      now = millis();
+    }
+  }
+}
+
+void setup()
+{
+  Serial.begin(115200);
+  while(!Serial) { }
+
+  Braccio.begin();
+
+  /* Ensure that servos don't block and hold too much. */
+  Braccio.speed(SLOW);
+  Braccio.disengage();
+
+
+  Serial.println("Calibrate Motor 5 - 1st arm segment should look vertically up.");
+  calibrate(5);
+  while(Braccio.isButtonPressed_ENTER()) { }
+
+  Serial.println("Calibrate Motor 4 - 2nd arm segment should look vertically up.");
+  calibrate(4);
+  while(Braccio.isButtonPressed_ENTER()) { }
+
+  Serial.println("Calibrate Motor 3 - 3rd arm segment should look vertically up.");
+  calibrate(3);
+  while(Braccio.isButtonPressed_ENTER()) { }
+
+  Serial.println("Calibrate Motor 2 - bottom of hand should be aligned with cable of motor 3.");
+  calibrate(2);
+  while(Braccio.isButtonPressed_ENTER()) { }
+
+  Serial.println("Calibrate Motor 1 - gripper should be firmly closed.");
+  calibrate(1);
+  while(Braccio.isButtonPressed_ENTER()) { }
+
+  Serial.println("Calibrate Motor 6 - front of motor 6 should be aligned with 0deg marking in plate.");
+  calibrate(6);
+  while(Braccio.isButtonPressed_ENTER()) { }
+}
+
+void loop()
+{
+
+}