From cb8adf7e0979cbc0cbc44c603e2ab9f30a703927 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 13 Mar 2024 17:23:40 +0100
Subject: [PATCH 1/2] waiting ack and idle_time

---
 src/Arduino_Alvik.cpp | 38 ++++++++++++++++++++++++++++++++------
 src/Arduino_Alvik.h   |  3 ++-
 src/definitions.h     |  5 ++++-
 3 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/src/Arduino_Alvik.cpp b/src/Arduino_Alvik.cpp
index 5c2d6e3..2cb55bd 100644
--- a/src/Arduino_Alvik.cpp
+++ b/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);
   }
@@ -57,6 +58,7 @@ int Arduino_Alvik::begin(const bool verbose, const uint8_t core){
   verbose_output = verbose;
 
   last_ack = 0;
+  waiting_ack = NO_ACK;
 
   version[0] = 0;
   version[1] = 0;
@@ -238,7 +240,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;
 
 
@@ -406,26 +413,44 @@ 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
+  int 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));
   }
 }
 
@@ -433,8 +458,9 @@ void Arduino_Alvik::move(const float distance, const uint8_t unit, const bool bl
   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));
   }
 }
 
diff --git a/src/Arduino_Alvik.h b/src/Arduino_Alvik.h
index 77823bf..ce3b5a9 100644
--- a/src/Arduino_Alvik.h
+++ b/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
diff --git a/src/definitions.h b/src/definitions.h
index 60e7d5a..f9791f8 100644
--- a/src/definitions.h
+++ b/src/definitions.h
@@ -29,11 +29,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
 

From 6caee2dc8b020a891e8c437c581b9265515603c1 Mon Sep 17 00:00:00 2001
From: Lucio Rossi <lucio.rossi75@gmail.com>
Date: Wed, 13 Mar 2024 17:41:14 +0100
Subject: [PATCH 2/2] wait_for_target wrong type to start_t

---
 src/Arduino_Alvik.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/Arduino_Alvik.cpp b/src/Arduino_Alvik.cpp
index 2cb55bd..46d237a 100644
--- a/src/Arduino_Alvik.cpp
+++ b/src/Arduino_Alvik.cpp
@@ -431,7 +431,7 @@ bool Arduino_Alvik::is_target_reached(){
 }
 
 void Arduino_Alvik::wait_for_target(const int idle_time){                                             //it is private
-  int start_t = millis();
+  unsigned long start_t = millis();
   
   while (true){
     if (((millis() - start_t) >= idle_time*1000) && is_target_reached()) {