駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myCan.cpp@6:20a32baeff79, 2018-12-18 (annotated)

Committer:
nakedt555
Date:
Tue Dec 18 21:51:13 2018 +0000
Revision:
6:20a32baeff79
Parent:
5:e678f1ac6cdc
Child:
7:b240464868e8
kita

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nakedt555 3:a45557a0dcb8 1 #include "myCan.h"
nakedt555 3:a45557a0dcb8 2
nakedt555 6:20a32baeff79 3
nakedt555 6:20a32baeff79 4 //This function run every 10 msec at main
nakedt555 6:20a32baeff79 5 void My_Can::loop(){
nakedt555 5:e678f1ac6cdc 6
nakedt555 5:e678f1ac6cdc 7 }
nakedt555 5:e678f1ac6cdc 8
nakedt555 6:20a32baeff79 9 void My_Can::receive_cb(){
nakedt555 5:e678f1ac6cdc 10 CANMessage canMsg;
nakedt555 5:e678f1ac6cdc 11 if(read(canMsg)){
nakedt555 5:e678f1ac6cdc 12 //ACKフレームの確認応答
nakedt555 5:e678f1ac6cdc 13 if (GetType(canMsg.id) == ACK_TYPE) {
nakedt555 5:e678f1ac6cdc 14 canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN);
nakedt555 5:e678f1ac6cdc 15 write(canMsg);
nakedt555 5:e678f1ac6cdc 16 }
nakedt555 5:e678f1ac6cdc 17
nakedt555 5:e678f1ac6cdc 18 //PINGフレームの確認応答
nakedt555 5:e678f1ac6cdc 19 if (GetType(canMsg.id) == PING_TYPE) {
nakedt555 5:e678f1ac6cdc 20 canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN);
nakedt555 5:e678f1ac6cdc 21 write(canMsg);
nakedt555 5:e678f1ac6cdc 22 }
nakedt555 5:e678f1ac6cdc 23
nakedt555 5:e678f1ac6cdc 24 switch(canMsg.data[0]){
nakedt555 5:e678f1ac6cdc 25 case ODOM_RESET:
nakedt555 5:e678f1ac6cdc 26
nakedt555 5:e678f1ac6cdc 27 break;
nakedt555 5:e678f1ac6cdc 28
nakedt555 5:e678f1ac6cdc 29 case ODOM_SET_INITIAL_XY:
nakedt555 5:e678f1ac6cdc 30 case ODOM_SET_INITIAL_ANGLE:
nakedt555 5:e678f1ac6cdc 31 check_initial_frame(canMsg.data);
nakedt555 5:e678f1ac6cdc 32 break;
nakedt555 5:e678f1ac6cdc 33 }
nakedt555 5:e678f1ac6cdc 34 }
nakedt555 5:e678f1ac6cdc 35 }
nakedt555 5:e678f1ac6cdc 36
nakedt555 5:e678f1ac6cdc 37 void My_Can::check_initial_frame(uint8_t *data){
nakedt555 5:e678f1ac6cdc 38 static uint32_t ts[2] = {0.0, 0.0};
nakedt555 5:e678f1ac6cdc 39 static can_odom_xy_t xy;
nakedt555 5:e678f1ac6cdc 40 static can_odom_angle_t angle;
nakedt555 6:20a32baeff79 41 static float radian;
nakedt555 5:e678f1ac6cdc 42
nakedt555 5:e678f1ac6cdc 43 switch(data[0]){
nakedt555 5:e678f1ac6cdc 44 case ODOM_SET_INITIAL_XY:
nakedt555 5:e678f1ac6cdc 45 for(int i = 0; i < 8; i++){
nakedt555 5:e678f1ac6cdc 46 xy.array[i] = data[i];
nakedt555 5:e678f1ac6cdc 47 }
nakedt555 5:e678f1ac6cdc 48 ts[0] = timer_.read_ms();
nakedt555 5:e678f1ac6cdc 49
nakedt555 5:e678f1ac6cdc 50 break;
nakedt555 5:e678f1ac6cdc 51
nakedt555 5:e678f1ac6cdc 52 case ODOM_SET_INITIAL_ANGLE:
nakedt555 5:e678f1ac6cdc 53 for(int i = 0; i < 8; i++){
nakedt555 6:20a32baeff79 54 angle.array[i] = data[i];
nakedt555 5:e678f1ac6cdc 55 }
nakedt555 6:20a32baeff79 56 radian = DecodeFloat(angle.data.angle);
nakedt555 5:e678f1ac6cdc 57 ts[1] = timer_.read_ms();
nakedt555 5:e678f1ac6cdc 58
nakedt555 5:e678f1ac6cdc 59 break;
nakedt555 5:e678f1ac6cdc 60 }
nakedt555 5:e678f1ac6cdc 61
nakedt555 6:20a32baeff79 62 if(ABS(ts[0] - ts[1]) > 50){
nakedt555 5:e678f1ac6cdc 63 return;
nakedt555 5:e678f1ac6cdc 64 }
nakedt555 5:e678f1ac6cdc 65
nakedt555 5:e678f1ac6cdc 66 reset_pose();
nakedt555 6:20a32baeff79 67 Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), radian);
nakedt555 5:e678f1ac6cdc 68 set_initial_pose(initialpose);
nakedt555 5:e678f1ac6cdc 69 set_court_color((angle.data.court_color == 0) ? COURT_RED : COURT_BLUE);
nakedt555 5:e678f1ac6cdc 70 led_toggle_.call();
nakedt555 3:a45557a0dcb8 71 }