Merge branch 'dev' of https://github.com/DaVinciBot/CoupeDeRobotique …

…into dev
DaVinciBot · Feb 1, 2024 · 410082f · 410082f
2 parents d2a853b + ca6c289
commit 410082f
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 5 deletions.
diff --git a/rasp/bot/Com.py b/rasp/bot/Com.py
@@ -310,6 +310,7 @@ class Command:
         EnablePid = b"\04"
         ResetPosition = b"\05"
         SetPID = b"\06"
+        SetHome = b'\07'
         Stop = b"\x7E"  # 7E = 126
         Invalid = b"\xFF"
 
@@ -352,14 +353,14 @@ def Go_To(
             + struct.pack("<f", pos.x)
             + struct.pack("<f", pos.y)
             + struct.pack("<?", direction)
-            + struct.pack("<H", max_speed)
+            + struct.pack("<B", max_speed)
             + struct.pack("<H", next_position_delay)
             + struct.pack("<H", action_error_auth)
             + struct.pack("<H", traj_precision)
-            + struct.pack("<H", correction_trajectory_speed)
-            + struct.pack("<H", acceleration_start_speed)
+            + struct.pack("<B", correction_trajectory_speed)
+            + struct.pack("<B", acceleration_start_speed)
             + struct.pack("<f", acceleration_distance)
-            + struct.pack("<H", deceleration_end_speed)
+            + struct.pack("<B", deceleration_end_speed)
             + struct.pack("<f", deceleration_distance)
         )
         # https://docs.python.org/3/library/struct.html#format-characters
@@ -454,13 +455,21 @@ def Enable_Pid(self, skip_queue=False):
             self.queue.append({self.Command.EnablePid: msg})
 
     @Logger
-    def Set_Home(self, skip_queue=False):
+    def Reset_Odo(self, skip_queue=False):
         msg = self.Command.ResetPosition
         if skip_queue or len(self.queue) == 0:
             self.queue.insert(0, {self.Command.ResetPosition: msg})
             self.send_bytes(msg)
         else:
             self.queue.append({self.Command.ResetPosition: msg})
+
+    def Set_Home(self, x, y, theta, *,skip_queue=False):
+        msg = self.Command.SetHome + struct.pack("<fff", x,y,theta)
+        if skip_queue or len(self.queue) == 0:
+            self.queue.insert(0, {self.Command.SetHome: msg})
+            self.send_bytes(msg)
+        else:
+            self.queue.append({self.Command.SetHome: msg})
 
     def Set_PID(self, Kp: float, Ki: float, Kd: float, skip_queue=False):
         msg = self.Command.SetPID + struct.pack("<fff", Kp, Ki, Kp)

diff --git a/teensy/include/messages.h b/teensy/include/messages.h
@@ -60,6 +60,14 @@ struct msg_Update_Position
     float theta;
 };
 
+struct msg_Set_Home
+{
+    byte command = SET_HOME;
+    float x;
+    float y;
+    float theta;
+};
+
 struct msg_Action_Finished
 {
     byte command = ACTION_FINISHED;

diff --git a/teensy/lib/actions/include/commands.h b/teensy/lib/actions/include/commands.h
@@ -9,6 +9,7 @@
 #define ENABLE_PID 4
 #define RESET_ODO 5
 #define SET_PID 6
+#define SET_HOME 7
 
 // two ways : 127 (Convention)
 #define NACK 127

diff --git a/teensy/src/main.cpp b/teensy/src/main.cpp
@@ -183,6 +183,19 @@ void set_pid(byte *msg, byte size)
   com->send_msg((byte *)&fin_msg, sizeof(msg_Action_Finished));
 }
 
+void set_home(byte *msg, byte size)
+{
+  msg_Set_Home *home_msg = (msg_Set_Home *)msg;
+  rolling_basis_ptr->X = home_msg->x;
+  rolling_basis_ptr->Y = home_msg->y;
+  rolling_basis_ptr->THETA = home_msg->theta;
+
+  msg_Action_Finished fin_msg;
+  fin_msg.action_id = SET_HOME;
+  com->send_msg((byte *)&fin_msg, sizeof(msg_Action_Finished));
+}
+
+
 void (*functions[256])(byte *msg, byte size);
 
 extern void handle_callback(Com *com);
@@ -223,6 +236,7 @@ void setup()
   functions[ENABLE_PID] = &enable_pid,
   functions[RESET_ODO] = &reset_odo,
   functions[SET_PID] = &set_pid,
+  functions[SET_HOME] = &set_home,
 
   Serial.begin(115200);