new ack management

src/Arduino_Alvik.cpp

@@ -45,6 +45,7 @@ void Arduino_Alvik::reset_hw(){
 }
 
 void Arduino_Alvik::wait_for_ack(){
+  waiting_ack = 0x00;
   while(last_ack != 0x00){
     delay(20);
   }
@@ -56,7 +57,8 @@ int Arduino_Alvik::begin(const bool verbose, const uint8_t core){
 
   verbose_output = verbose;
 
-  last_ack = 255;
+  last_ack = 0;
+  waiting_ack = NO_ACK;
 
   version[0] = 0;
   version[1] = 0;
@@ -242,7 +244,12 @@ int Arduino_Alvik::parse_message(){
   switch(code){
     // get ack code
     case 'x':
-      packeter->unpacketC1B(code, last_ack);
+      if (waiting_ack == NO_ACK){
+        packeter->unpacketC1B(code, last_ack);
+        last_ack = 0x00;
+      } else {
+        packeter->unpacketC1B(code, last_ack);
+      }
       break;
 
 
@@ -410,35 +417,54 @@ void Arduino_Alvik::reset_pose(const float x, const float y, const float theta,
 }
 
 bool Arduino_Alvik::is_target_reached(){
-  if ((last_ack != 'M') && (last_ack != 'R')){
+
+  if (waiting_ack == NO_ACK){
+    return true;
+  }
+
+  if (last_ack != waiting_ack){
     delay(50);
     return false;
   }
   msg_size = packeter->packetC1B('X', 'K');
   uart->write(packeter->msg, msg_size);
+  waiting_ack = NO_ACK;
+  last_ack = 0x00;
   delay(200);
   return true;
 }
 
-void Arduino_Alvik::wait_for_target(){                                             //it is private
-  while (!is_target_reached()){}
+void Arduino_Alvik::wait_for_target(const int idle_time){                                             //it is private
+  unsigned long start_t = millis();
+
+  while (true){
+    if (((millis() - start_t) >= idle_time*1000) && is_target_reached()) {
+      break;
+    } else
+    {
+      delay(100);
+    }
+
+  }
 }
 
 void Arduino_Alvik::rotate(const float angle, const uint8_t unit, const bool blocking){
   delay(200);
   msg_size = packeter->packetC1F('R', convert_angle(angle, unit, DEG));
   uart->write(packeter->msg, msg_size);
+  waiting_ack = 'R';
   if (blocking){
-    wait_for_target();
+    wait_for_target(round(angle/MOTOR_CONTROL_DEG_S));
   }
 }
 
 void Arduino_Alvik::move(const float distance, const uint8_t unit, const bool blocking){
   delay(200);
   msg_size = packeter->packetC1F('G', convert_distance(distance, unit, MM));
   uart->write(packeter->msg, msg_size);
+  waiting_ack = 'M';
   if (blocking){
-    wait_for_target();
+    wait_for_target(round(distance/MOTOR_CONTROL_MM_S));
   }
 }
 

src/Arduino_Alvik.h

@@ -36,6 +36,7 @@ class Arduino_Alvik{
     uint8_t msg_size;
 
     uint8_t last_ack;
+    uint8_t waiting_ack;
 
     float converted_angular;
 
@@ -96,7 +97,7 @@ class Arduino_Alvik{
 
     void get_touch();                                                   // service function to parse touch
     void set_leds();                                                    // service function to set leds by a byte
-    void wait_for_target();                                             // service function that wait for ack
+    void wait_for_target(const int idle_time);                                             // service function that wait for ack
 
     float limit(float value, const float min, const float max);         // limit a value
     float normalize(float value, const float min, const float max);     // normalize a value

src/definitions.h

@@ -32,11 +32,14 @@
 
 #define CHARGE_THRESHOLD 97
 
+#define NO_ACK 0xFF
+
 const float WHEEL_DIAMETER_MM = 34.0;
 const float WHEEL_TRACK_MM = 89.0;
 const float MOTOR_MAX_RPM = 70.0;
 const float ROBOT_MAX_DEG_S = 6*(2*MOTOR_MAX_RPM*WHEEL_DIAMETER_MM)/WHEEL_TRACK_MM;
-
+const float MOTOR_CONTROL_MM_S = 100.0;
+const float MOTOR_CONTROL_DEG_S = 100.0;
 
 // unit conversion constants