Cleanup.

Author: aentinger

examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino

@@ -19,24 +19,6 @@
  * TYPEDEF
  **************************************************************************************/
 
-enum states {
-  SHOULDER,
-  ELBOW,
-  WRIST,
-  PINCH
-};
-
-// Values of clicked input keys returned by Braccio.getKey() function (defined inside the library).
-enum KEYS
-{
-  UP = 4,
-  DOWN = 5,
-  LEFT = 1,
-  RIGHT = 2,
-  CLICK = 3, // click of joystick
-  ENTER = 6,
-};
-
 // IDs of the displayed directional UI buttons
 enum BUTTONS {
   BTN_UP = 1,
@@ -54,129 +36,13 @@ static const char *direction_btnm_map[] = {" ", LV_SYMBOL_UP, " ", "\n",
  **************************************************************************************/
 
 // Variables
-float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 190.0};
-float angles[6]; // All motors current angles
-
-// Braccio ++ joints
-auto gripper    = Braccio.get(1);
-auto wristRoll  = Braccio.get(2);
-auto wristPitch = Braccio.get(3);
-auto elbow      = Braccio.get(4);
-auto shoulder   = Braccio.get(5);
-auto base       = Braccio.get(6);
-
-char * jointsPair[] = {"Shoulder", "Elbow", "Wrist", "Pinch"};
-
-int state = SHOULDER;
+static float const HOME_POS[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0};
 
 lv_obj_t * direction_btnm;  // Direction button matrix
 lv_obj_t * label; // Label
 
 ManualControlApp app;
 
-// Function
-void moveJoints(uint32_t btnID) {
-  switch (state) {
-    case SHOULDER:
-      switch (btnID) {
-        case 4: shoulder.move().to(angles[4] - 10.0); break;
-        case 1: base.move().to(angles[5] - 10.0);    break;
-        case 2: base.move().to(angles[5] + 10.0);    break;
-        case 5: shoulder.move().to(angles[4] + 10.0); break;
-        default: break;
-      }
-      break;
-    case ELBOW:
-      switch (btnID) {
-        case 4: elbow.move().to(angles[3] - 10.0); break;
-        case 5: elbow.move().to(angles[3] + 10.0); break;
-        default: break;
-      }
-      break;
-    case WRIST:
-      switch (btnID) {
-        case 4: wristPitch.move().to(angles[2] - 10.0); break;
-        case 1: wristRoll.move().to(angles[1] - 10.0);  break;
-        case 2: wristRoll.move().to(angles[1] + 10.0);  break;
-        case 5: wristPitch.move().to(angles[2] + 10.0); break;
-        default: break;
-      }
-      break;
-    case PINCH:
-      switch (btnID) {
-        case 1: gripper.move().to(angles[0] + 10.0); break;
-        case 2: gripper.move().to(angles[0] - 10.0); break;      
-        default: break;
-      }
-    default:
-      break;
-  }
-}
-
-void updateButtons(uint32_t key)
-{
-  if (key == UP){
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_selected_btn(direction_btnm, BTN_UP);
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_CHECKED);
-    Braccio.lvgl_unlock();
-  }
-  else if (key == DOWN){
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_selected_btn(direction_btnm, BTN_DOWN);
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_CHECKED);
-    Braccio.lvgl_unlock();
-  }
-  else if (key == LEFT) {
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_selected_btn(direction_btnm, BTN_LEFT);
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_CHECKED);
-    Braccio.lvgl_unlock();
-  }
-  else if (key == RIGHT){
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_selected_btn(direction_btnm, BTN_RIGHT);
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_CHECKED);
-    Braccio.lvgl_unlock();
-  }
-  else {
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_selected_btn(direction_btnm, NULL);
-    Braccio.lvgl_unlock();
-  }
-/*
-  if (state == ELBOW){
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN);
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN);
-    Braccio.lvgl_unlock();
-  }
-  else if (state == PINCH){
-    Braccio.lvgl_lock();
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_HIDDEN);
-    lv_btnmatrix_set_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_HIDDEN);
-    Braccio.lvgl_unlock();
-  }
-  else{
-    Braccio.lvgl_lock();
-    lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_UP, LV_BTNMATRIX_CTRL_HIDDEN);
-    lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_DOWN, LV_BTNMATRIX_CTRL_HIDDEN);
-    lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_LEFT, LV_BTNMATRIX_CTRL_HIDDEN);
-    lv_btnmatrix_clear_btn_ctrl(direction_btnm, BTN_RIGHT, LV_BTNMATRIX_CTRL_HIDDEN);
-    Braccio.lvgl_unlock();
-  }
-  */
-  /*
-  Braccio.lvgl_lock();
-  lv_label_set_text(label, jointsPair[state]);
-  Braccio.lvgl_unlock();
-  */
-}
-
-// Event Handlers
-
-
-
 // Screens functions
 
 void directionScreen(void)
@@ -217,7 +83,7 @@ void directionScreen(void)
   lv_obj_set_width(label, 240);
   lv_obj_set_style_text_align(label, LV_TEXT_ALIGN_CENTER, 0);
   lv_obj_align(label, LV_ALIGN_CENTER, 0, 0);
-  lv_label_set_text(label, jointsPair[state]);
+  lv_label_set_text(label, "");
 
   Braccio.lvgl_unlock();
 }
@@ -231,8 +97,10 @@ void setup()
     /* Configure Braccio. */
     Braccio.speed(MEDIUM);
     /* Move to home position. */
-    Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
+    Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
     delay(500);
+    /* Init state. */
+    app.update(Button::None);
     /* Enable buttons. */
     Braccio.lvgl_lock();
     lv_btnmatrix_clear_btn_ctrl(direction_btnm, 1, LV_BTNMATRIX_CTRL_DISABLED);
@@ -241,8 +109,6 @@ void setup()
     lv_btnmatrix_clear_btn_ctrl(direction_btnm, 7, LV_BTNMATRIX_CTRL_DISABLED);
     Braccio.lvgl_unlock();
   }
-
-  app.update(Button::None);
 }
 
 void loop()