Fix: Prevent sample array (values) from overflowing.

Author: aentinger

examples/Braccio_Learn_and_Repeat/Braccio_Learn_and_Repeat.ino

@@ -13,9 +13,12 @@ enum states {
 
 int state = ZERO_POSITION;
 
-float values[10000];
+static int const MAX_SAMPLES = 6*1000*2; /* 20 seconds. */
+
+float values[MAX_SAMPLES];
 float* idx = values;
 float* final_idx = 0;
+int sample_cnt = 0;
 float homePos[6] = {157.5, 157.5, 157.5, 157.5, 157.5, 90.0};
 
 static lv_obj_t * counter;
@@ -42,6 +45,7 @@ static void eventHandlerMenu(lv_event_t * e) {
     }
 
     idx = values;
+    sample_cnt = 0;
 
     switch (id) {
       case 0: // if the button pressed is the first one
@@ -141,25 +145,32 @@ void setup() {
 
 void loop() {
   if (state == RECORD) {
+
+    /* Check if we still have space for samples. */
+    if (sample_cnt >= MAX_SAMPLES) {
+      state = ZERO_POSITION;
+      Serial.println("ZERO_POSITION");
+      btnm_map[0] = "RECORD"; // reset the label of the first button back to "RECORD"
+      lv_btnmatrix_set_btn_ctrl(btnm, 0, LV_BTNMATRIX_CTRL_CHECKABLE);
+    }
+    /* Capture those samples. */
     Braccio.positions(idx);
     idx += 6;
+    sample_cnt += 6;
   }
   if (state == REPLAY) {
     Braccio.moveTo(idx[0], idx[1], idx[2], idx[3], idx[4], idx[5]);
     idx += 6;
+    sample_cnt += 6;
     if (idx >= final_idx) {
       Serial.println("REPLAY done");
       state = ZERO_POSITION;
       btnm_map[2] = "REPLAY"; // reset the label of the first button back to "REPLAY"
       lv_btnmatrix_set_btn_ctrl(btnm, 2, LV_BTNMATRIX_CTRL_CHECKED);
     }
   }
-  if (idx - values >= sizeof(values)) {
-    Serial.println("ZERO_POSITION");
-    state = ZERO_POSITION;
-  }
   delay(100);
   if (state != ZERO_POSITION) {
-    lv_label_set_text_fmt(counter, "Counter: %d" , idx - values);
+    lv_label_set_text_fmt(counter, "Counter: %d" , sample_cnt);
   }
 }

examples/Braccio_SLIP_Control/Braccio_SLIP_Control.ino

@@ -0,0 +1,73 @@
+/*
+ * @brief Activating the "MOVE" button by pressing
+ * the joystick enables a waving motion of the arm.
+ */
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
+#include <Braccio++.h>
+
+#include <PacketSerial.h>
+
+/**************************************************************************************
+ * TYPEDEF
+ **************************************************************************************/
+
+union Braccio2ROSMessage
+{
+  struct __attribute__((packed, aligned(4)))
+  {
+    float joint_position_deg[SmartServoClass::NUM_MOTORS];
+  } data;
+  uint8_t buf[24];
+};
+static_assert(sizeof(Braccio2ROSMessage) == 24);
+
+/**************************************************************************************
+ * FUNCTION DECLARATION
+ **************************************************************************************/
+
+void onPacketReceived(const uint8_t* buffer, size_t size);
+
+/**************************************************************************************
+ * GLOBAL VARIABLES
+ **************************************************************************************/
+
+SLIPPacketSerial packet_serial;
+
+/**************************************************************************************
+ * SETUP/LOOP
+ **************************************************************************************/
+
+void setup()
+{
+  Braccio.begin();
+
+  packet_serial.begin(115200);
+  packet_serial.setPacketHandler(&onPacketReceived);
+}
+
+void loop()
+{
+  packet_serial.update();
+
+  static auto prev = millis();
+  if (auto const now = millis(); (now - prev) > 50)
+  {
+    prev = now;
+    Braccio2ROSMessage msg;
+    Braccio.positions(msg.data.joint_position_deg);
+    packet_serial.send(msg.buf, sizeof(msg.buf));
+  }
+}
+
+/**************************************************************************************
+ * FUNCTION DEFINITION
+ **************************************************************************************/
+
+void onPacketReceived(const uint8_t* buffer, size_t size)
+{
+  /* TODO */
+}