Diff: main.cpp

Revision:

1:2720d0e8b2f1

Parent:

0:fe598340f74c

Child:

2:626a20dd7987

diff -r fe598340f74c -r 2720d0e8b2f1 main.cpp
--- a/main.cpp	Mon Aug 23 17:12:44 2021 +0000
+++ b/main.cpp	Tue Aug 24 07:06:27 2021 +0000
@@ -42,26 +42,19 @@
 
 void read_pc_thread()
 {
-    int i = 0;
     while (true) {
         commander.receive_pc_process();
-        UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose();
-        leds = ref_pose.x % 16;
-        i++;
-        pc.writeData(ref_pose.x, UsusamaProtocol::COMMAND_POSE_X);
-        pc.writeData(ref_pose.y, UsusamaProtocol::COMMAND_POSE_Y);
-        pc.writeData(ref_pose.theta, UsusamaProtocol::COMMAND_POSE_THETA);
+        leds = commander.getReferencePose().x;
         ThisThread::sleep_for(53);
     }
 }
 
 void write_pc_thread()
 {
+    Pose2D current_pose(0,0,0);
     while (true) {
-        UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose();
-        pc.writeData(ref_pose.x, UsusamaProtocol::COMMAND_POSE_X);
-        pc.writeData(ref_pose.y, UsusamaProtocol::COMMAND_POSE_Y);
-        pc.writeData(ref_pose.theta, UsusamaProtocol::COMMAND_POSE_THETA);
+        current_pose = ususama.GetPose();
+        commander.write_robot_state(current_pose.x, current_pose.y, current_pose.theta);
         ThisThread::sleep_for(47);
     }
 }
@@ -90,6 +83,10 @@
         // todo: read using timer
         thread_sleep_for(10);
         
+        
+        UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose();
+        
+        
         ususama.ComputePose();
         ususama.ControlPosition( way_generator.GetPose(i_way) );