Diff: myCan.cpp

Revision:

8:80708bacb5b5

Parent:

7:b240464868e8

Child:

9:b20145b7e1ba

--- a/myCan.cpp	Tue Dec 18 22:06:34 2018 +0000
+++ b/myCan.cpp	Thu Dec 20 20:54:35 2018 +0000
@@ -3,7 +3,13 @@
 
 //This function run every 10 msec at main
 void My_Can::loop(){
-
+    static uint32_t counter;
+    if(enable_send_odom_){
+        if((counter++ % 2) == 0){
+            send_odom(); 
+            led_toggle_.call();   
+        }
+    }
 }
 
 void My_Can::receive_cb(){
@@ -23,11 +29,13 @@
         
         switch(canMsg.data[0]){
         case ODOM_RESET:
-            
+            enable_send_odom_ = false;
+            reset_odometry();
             break;  
             
         case ODOM_SET_INITIAL_XY:
         case ODOM_SET_INITIAL_ANGLE:
+        case ODOM_SET_CONFIG:
             check_initial_frame(canMsg.data);
             break;
         }
@@ -35,13 +43,14 @@
 }
 
 void My_Can::check_initial_frame(uint8_t *data){
-    static uint32_t ts[2] = {0.0, 0.0};
+    static uint32_t ts[3] = {0.0, 0.0, 0.0};
     static can_odom_xy_t xy;
     static can_odom_angle_t angle;
+    static can_odom_config_t config;
     
     switch(data[0]){
     case ODOM_SET_INITIAL_XY:
-        for(int i = 0; i < 8; i++){
+        for(int i = 0; i < 6; i++){
             xy.array[i] = data[i];   
         }
         ts[0] = timer_.read_ms();
@@ -49,21 +58,51 @@
         break;
         
     case ODOM_SET_INITIAL_ANGLE:
-        for(int i = 0; i < 8; i++){
+        for(int i = 0; i < 6; i++){
             angle.array[i] = data[i];  
         }
         ts[1] = timer_.read_ms();
     
-        break;   
+        break;
+    
+    case ODOM_SET_CONFIG:
+        for(int i = 0; i < 3; i++){
+            config.array[i] = data[i];  
+        }
+        ts[2] = timer_.read_ms();
+        
+        break;
     }
     
-    if(ABS(ts[0] - ts[1]) > 50){
+    if(ABS(ts[0] - ts[1]) > 50 && ABS(ts[1] - ts[2]) > 50 && ABS(ts[0] - ts[2]) > 50){
         return;
     }
     
-    reset_pose();
+    reset_odometry();
+    
     Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle));
     set_initial_pose(initialpose);
-    set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE);
-    led_toggle_.call(); 
+    set_court_color((config.data.court_color == 0) ? COURT_RED : COURT_BLUE);
+    set_enable_oled((config.data.enable_oled == 0) ? false : true);
+    
+    enable_send_odom_ = true; //CAN送信許可
+    amcl_initialize_.call();
+}
+
+void My_Can::send_odom(){
+    can_odom_xy_t odom_xy;
+    can_odom_angle_t odom_angle;
+
+    odom_xy.data.frameName = ODOM_DATA_XY;
+    odom_angle.data.frameName = ODOM_DATA_ANGLE; 
+    
+    Vec3f pos = get_world();
+    
+    odom_xy.data.x = EncodeFixedNumber(pos.x());
+    odom_xy.data.y = EncodeFixedNumber(pos.y());
+    EncodeFloat(pos.angle(), odom_angle.data.angle);
+
+    write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_xy.array, 6));
+    wait_ms(1);
+    write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_angle.array, 6));
 }
\ No newline at end of file