Revamping example to steer every servo through the full range of motion.

Author: aentinger

examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino

@@ -3,18 +3,59 @@
  * angle for each smart servo motor.
  */
 
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
 #include <Braccio++.h>
 
+/**************************************************************************************
+ * FUNCTIONS
+ **************************************************************************************/
+
+void test_motor(int const id)
+{
+  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);
+
+  Serial.print("Drive to start . ");
+  Braccio.get(id).move().to(0.0f).in(1s);
+  Serial.println("OK.");
+  delay(1500);
+
+  for (float target_angle = 0.0f; target_angle < 315.0f; target_angle += 1.0f)
+  {
+    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){}
 
-  /* Call Braccio.begin() for default menu
-   * or pass a function for custom menu.
-   */
   Braccio.begin();
-
   Serial.println("Testing motor angular movement!");
 }
 
@@ -24,25 +65,13 @@ void loop()
   Serial.println(">> ");
 
   while((Serial.available() <= 0)) { }
-  int const selected_motor = Serial.parseInt();
+  int const motor_id = Serial.parseInt();
   while(Serial.read() != '\n') { }
 
-  if (selected_motor < 1 || selected_motor > 6) {
+  if (motor_id < 1 || motor_id > 6) {
     Serial.println("Error, wrong motor id, choose motor id between 1 and 6");
     return;
   }
 
-  float const ANGLE_START     = 0.0;
-  float const ANGLE_STOP      = 180.0;
-  float const ANGLE_INCREMENT = 10.0;
-
-  for (float angle = ANGLE_START; angle <= ANGLE_STOP; angle += ANGLE_INCREMENT)
-  {
-    Braccio.move(selected_motor).to(angle);
-    Serial.print("Target angle: " + String(angle));
-    Serial.print(" | ");
-    Serial.print("Current angle: " + String(Braccio.get(selected_motor).position()));
-    Serial.println();
-    delay(100);
-  }
+  test_motor(motor_id);
 }