Diff: myCan.cpp

Revision:

5:e678f1ac6cdc

Parent:

3:a45557a0dcb8

Child:

6:20a32baeff79

--- a/myCan.cpp	Wed Dec 12 03:35:52 2018 +0000
+++ b/myCan.cpp	Tue Dec 18 21:00:30 2018 +0000
@@ -1,6 +1,72 @@
 #include "myCan.h"
 
+void receive_func_cb(){
+
+}
+
 //This function run every 10 msec at main
 void My_Can::loop(){
+    receive();
+}
 
+void My_Can::receive(){
+    CANMessage canMsg;
+    if(read(canMsg)){         
+        //ACKフレームの確認応答
+        if (GetType(canMsg.id) == ACK_TYPE) {
+            canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN);
+            write(canMsg);
+        }
+        
+        //PINGフレームの確認応答
+        if (GetType(canMsg.id) == PING_TYPE) {
+            canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN);
+            write(canMsg);
+        }
+        
+        switch(canMsg.data[0]){
+        case ODOM_RESET:
+            
+            break;  
+            
+        case ODOM_SET_INITIAL_XY:
+        case ODOM_SET_INITIAL_ANGLE:
+            check_initial_frame(canMsg.data);
+            break;
+        }
+    }   
+}
+
+void My_Can::check_initial_frame(uint8_t *data){
+    static uint32_t ts[2] = {0.0, 0.0};
+    static can_odom_xy_t xy;
+    static can_odom_angle_t angle;
+    
+    switch(data[0]){
+    case ODOM_SET_INITIAL_XY:
+        for(int i = 0; i < 8; i++){
+            xy.array[i] = data[i];   
+        }
+        ts[0] = timer_.read_ms();
+        
+        break;
+        
+    case ODOM_SET_INITIAL_ANGLE:
+        for(int i = 0; i < 8; i++){
+            angle.array[i] = data[i];   
+        }
+        ts[1] = timer_.read_ms();
+    
+        break;   
+    }
+    
+    if(ABS(ts[0] - ts[1]) > 100){
+        return;
+    }
+    
+    reset_pose();
+    Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), angle.data.angle);
+    set_initial_pose(initialpose);
+    set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE);
+    led_toggle_.call(); 
 }
\ No newline at end of file