駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myCan.cpp@5:e678f1ac6cdc, 2018-12-18 (annotated)

Committer:
nakedt555
Date:
Tue Dec 18 21:00:30 2018 +0000
Revision:
5:e678f1ac6cdc
Parent:
3:a45557a0dcb8
Child:
6:20a32baeff79
before can it

Who changed what in which revision?

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