Diff: myCan.cpp

Revision:

6:20a32baeff79

Parent:

5:e678f1ac6cdc

Child:

7:b240464868e8

diff -r e678f1ac6cdc -r 20a32baeff79 myCan.cpp
--- a/myCan.cpp	Tue Dec 18 21:00:30 2018 +0000
+++ b/myCan.cpp	Tue Dec 18 21:51:13 2018 +0000
@@ -1,15 +1,12 @@
 #include "myCan.h"
 
-void receive_func_cb(){
+
+//This function run every 10 msec at main
+void My_Can::loop(){
 
 }
 
-//This function run every 10 msec at main
-void My_Can::loop(){
-    receive();
-}
-
-void My_Can::receive(){
+void My_Can::receive_cb(){
     CANMessage canMsg;
     if(read(canMsg)){         
         //ACKフレームの確認応答
@@ -41,6 +38,7 @@
     static uint32_t ts[2] = {0.0, 0.0};
     static can_odom_xy_t xy;
     static can_odom_angle_t angle;
+    static float radian;
     
     switch(data[0]){
     case ODOM_SET_INITIAL_XY:
@@ -53,19 +51,20 @@
         
     case ODOM_SET_INITIAL_ANGLE:
         for(int i = 0; i < 8; i++){
-            angle.array[i] = data[i];   
+            angle.array[i] = data[i];  
         }
+        radian = DecodeFloat(angle.data.angle);
         ts[1] = timer_.read_ms();
     
         break;   
     }
     
-    if(ABS(ts[0] - ts[1]) > 100){
+    if(ABS(ts[0] - ts[1]) > 50){
         return;
     }
     
     reset_pose();
-    Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), angle.data.angle);
+    Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), radian);
     set_initial_pose(initialpose);
     set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE);
     led_toggle_.call();