駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myCan.cpp@12:f726eb78b54c, 2019-08-30 (annotated)

Committer:
nakedt555
Date:
Fri Aug 30 07:55:38 2019 +0000
Revision:
12:f726eb78b54c
Parent:
9:b20145b7e1ba 
aaa

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 8:80708bacb5b5 6 static uint32_t counter;
nakedt555 8:80708bacb5b5 7 if(enable_send_odom_){
nakedt555 8:80708bacb5b5 8 if((counter++ % 2) == 0){
nakedt555 8:80708bacb5b5 9 send_odom();
nakedt555 8:80708bacb5b5 10 led_toggle_.call();
nakedt555 8:80708bacb5b5 11 }
nakedt555 8:80708bacb5b5 12 }
nakedt555 5:e678f1ac6cdc 13 }
nakedt555 5:e678f1ac6cdc 14
nakedt555 6:20a32baeff79 15 void My_Can::receive_cb(){
nakedt555 5:e678f1ac6cdc 16 CANMessage canMsg;
nakedt555 5:e678f1ac6cdc 17 if(read(canMsg)){
nakedt555 5:e678f1ac6cdc 18 //ACKフレームの確認応答
nakedt555 5:e678f1ac6cdc 19 if (GetType(canMsg.id) == ACK_TYPE) {
nakedt555 5:e678f1ac6cdc 20 canMsg.id = CreateSid(ACK_TYPE, ID_ODOM, ID_MAIN);
nakedt555 5:e678f1ac6cdc 21 write(canMsg);
nakedt555 5:e678f1ac6cdc 22 }
nakedt555 5:e678f1ac6cdc 23
nakedt555 5:e678f1ac6cdc 24 //PINGフレームの確認応答
nakedt555 5:e678f1ac6cdc 25 if (GetType(canMsg.id) == PING_TYPE) {
nakedt555 5:e678f1ac6cdc 26 canMsg.id = CreateSid(PING_TYPE, ID_ODOM, ID_MAIN);
nakedt555 5:e678f1ac6cdc 27 write(canMsg);
nakedt555 5:e678f1ac6cdc 28 }
nakedt555 5:e678f1ac6cdc 29
nakedt555 5:e678f1ac6cdc 30 switch(canMsg.data[0]){
nakedt555 5:e678f1ac6cdc 31 case ODOM_RESET:
nakedt555 8:80708bacb5b5 32 enable_send_odom_ = false;
nakedt555 8:80708bacb5b5 33 reset_odometry();
nakedt555 5:e678f1ac6cdc 34 break;
nakedt555 5:e678f1ac6cdc 35
nakedt555 5:e678f1ac6cdc 36 case ODOM_SET_INITIAL_XY:
nakedt555 5:e678f1ac6cdc 37 case ODOM_SET_INITIAL_ANGLE:
nakedt555 8:80708bacb5b5 38 case ODOM_SET_CONFIG:
nakedt555 5:e678f1ac6cdc 39 check_initial_frame(canMsg.data);
nakedt555 5:e678f1ac6cdc 40 break;
nakedt555 5:e678f1ac6cdc 41 }
nakedt555 5:e678f1ac6cdc 42 }
nakedt555 5:e678f1ac6cdc 43 }
nakedt555 5:e678f1ac6cdc 44
nakedt555 5:e678f1ac6cdc 45 void My_Can::check_initial_frame(uint8_t *data){
nakedt555 8:80708bacb5b5 46 static uint32_t ts[3] = {0.0, 0.0, 0.0};
nakedt555 5:e678f1ac6cdc 47 static can_odom_xy_t xy;
nakedt555 5:e678f1ac6cdc 48 static can_odom_angle_t angle;
nakedt555 8:80708bacb5b5 49 static can_odom_config_t config;
nakedt555 5:e678f1ac6cdc 50
nakedt555 5:e678f1ac6cdc 51 switch(data[0]){
nakedt555 5:e678f1ac6cdc 52 case ODOM_SET_INITIAL_XY:
nakedt555 8:80708bacb5b5 53 for(int i = 0; i < 6; i++){
nakedt555 5:e678f1ac6cdc 54 xy.array[i] = data[i];
nakedt555 5:e678f1ac6cdc 55 }
nakedt555 5:e678f1ac6cdc 56 ts[0] = timer_.read_ms();
nakedt555 5:e678f1ac6cdc 57
nakedt555 5:e678f1ac6cdc 58 break;
nakedt555 5:e678f1ac6cdc 59
nakedt555 5:e678f1ac6cdc 60 case ODOM_SET_INITIAL_ANGLE:
nakedt555 8:80708bacb5b5 61 for(int i = 0; i < 6; i++){
nakedt555 6:20a32baeff79 62 angle.array[i] = data[i];
nakedt555 5:e678f1ac6cdc 63 }
nakedt555 5:e678f1ac6cdc 64 ts[1] = timer_.read_ms();
nakedt555 5:e678f1ac6cdc 65
nakedt555 8:80708bacb5b5 66 break;
nakedt555 8:80708bacb5b5 67
nakedt555 8:80708bacb5b5 68 case ODOM_SET_CONFIG:
nakedt555 8:80708bacb5b5 69 for(int i = 0; i < 3; i++){
nakedt555 8:80708bacb5b5 70 config.array[i] = data[i];
nakedt555 8:80708bacb5b5 71 }
nakedt555 8:80708bacb5b5 72 ts[2] = timer_.read_ms();
nakedt555 8:80708bacb5b5 73
nakedt555 8:80708bacb5b5 74 break;
nakedt555 5:e678f1ac6cdc 75 }
nakedt555 5:e678f1ac6cdc 76
nakedt555 8:80708bacb5b5 77 if(ABS(ts[0] - ts[1]) > 50 && ABS(ts[1] - ts[2]) > 50 && ABS(ts[0] - ts[2]) > 50){
nakedt555 5:e678f1ac6cdc 78 return;
nakedt555 5:e678f1ac6cdc 79 }
nakedt555 5:e678f1ac6cdc 80
nakedt555 8:80708bacb5b5 81 reset_odometry();
nakedt555 8:80708bacb5b5 82
nakedt555 7:b240464868e8 83 Vec3f initialpose(DecodeFixedNumber(xy.data.x), DecodeFixedNumber(xy.data.y), DecodeFloat(angle.data.angle));
nakedt555 5:e678f1ac6cdc 84 set_initial_pose(initialpose);
nakedt555 8:80708bacb5b5 85 set_court_color((config.data.court_color == 0) ? COURT_RED : COURT_BLUE);
nakedt555 8:80708bacb5b5 86 set_enable_oled((config.data.enable_oled == 0) ? false : true);
nakedt555 8:80708bacb5b5 87
nakedt555 8:80708bacb5b5 88 enable_send_odom_ = true; //CAN送信許可
nakedt555 8:80708bacb5b5 89 amcl_initialize_.call();
nakedt555 8:80708bacb5b5 90 }
nakedt555 8:80708bacb5b5 91
nakedt555 8:80708bacb5b5 92 void My_Can::send_odom(){
nakedt555 8:80708bacb5b5 93 can_odom_xy_t odom_xy;
nakedt555 8:80708bacb5b5 94 can_odom_angle_t odom_angle;
nakedt555 8:80708bacb5b5 95
nakedt555 8:80708bacb5b5 96 odom_xy.data.frameName = ODOM_DATA_XY;
nakedt555 8:80708bacb5b5 97 odom_angle.data.frameName = ODOM_DATA_ANGLE;
nakedt555 8:80708bacb5b5 98
nakedt555 8:80708bacb5b5 99 Vec3f pos = get_world();
nakedt555 8:80708bacb5b5 100
nakedt555 8:80708bacb5b5 101 odom_xy.data.x = EncodeFixedNumber(pos.x());
nakedt555 8:80708bacb5b5 102 odom_xy.data.y = EncodeFixedNumber(pos.y());
nakedt555 9:b20145b7e1ba 103 EncodeFloat(-pos.angle(), odom_angle.data.angle);//注意：マイナスをつけてる
nakedt555 8:80708bacb5b5 104
nakedt555 8:80708bacb5b5 105 write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_xy.array, 6));
nakedt555 8:80708bacb5b5 106 wait_ms(1);
nakedt555 8:80708bacb5b5 107 write(CANMessage(CreateSid(NORMAL_TYPE, ID_ODOM, ID_MAIN), odom_angle.array, 6));
nakedt555 3:a45557a0dcb8 108 }