春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_ashi_3_19

3/19

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

can/can.cpp@11:9db93bce4eef, 2019-03-19 (annotated)

Committer:
yuki0701
Date:
Tue Mar 19 02:30:34 2019 +0000
Revision:
11:9db93bce4eef
Parent:
9:7667dcfc7ce5 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:c61c6e4775ca 1 #include "mbed.h"
la00noix 0:c61c6e4775ca 2 #include "PathFollowing.h"
la00noix 0:c61c6e4775ca 3 #include "movement.h"
la00noix 0:c61c6e4775ca 4 #include "maxonsetting.h"
la00noix 0:c61c6e4775ca 5 #include "manual.h"
la00noix 0:c61c6e4775ca 6 #include "can.h"
la00noix 0:c61c6e4775ca 7
la00noix 9:7667dcfc7ce5 8 CAN can1(p30,p29);
la00noix 0:c61c6e4775ca 9 Ticker can_ticker; //can用ticker
la00noix 0:c61c6e4775ca 10
la00noix 9:7667dcfc7ce5 11 DigitalOut cansend_led(LED1); //cansend -> on
la00noix 9:7667dcfc7ce5 12 DigitalOut canread_led(LED2); //canread -> on
la00noix 0:c61c6e4775ca 13
la00noix 8:2ba338b4590e 14 int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
yuki0701 11:9db93bce4eef 15 int usw_flag;
la00noix 8:2ba338b4590e 16
la00noix 0:c61c6e4775ca 17 void can_readsend()
la00noix 0:c61c6e4775ca 18 {
la00noix 0:c61c6e4775ca 19 CANMessage msg;
yuki0701 11:9db93bce4eef 20 // printf("readsend\n\r");
yuki0701 3:8a0faa3b08c3 21
la00noix 0:c61c6e4775ca 22 if(can1.read(msg)) {
yuki0701 11:9db93bce4eef 23 //printf("read\n\r");
la00noix 8:2ba338b4590e 24
la00noix 0:c61c6e4775ca 25 canread_led = 1;
yuki0701 3:8a0faa3b08c3 26
yuki0701 3:8a0faa3b08c3 27 if(msg.id == 1) { //from main
yuki0701 3:8a0faa3b08c3 28
la00noix 6:26724c287387 29 id1_value[0] = (msg.data[0]>>6)%4; //decide wait/auto/manual(0~2)
la00noix 6:26724c287387 30 id1_value[1] = msg.data[1]; //angle of left joystick(0~359)
la00noix 0:c61c6e4775ca 31 id1_value[2] = msg.data[2];
la00noix 6:26724c287387 32 id1_value[3] = (msg.data[0]>>5)%2; //BOTTONR1 off/on(0 or 1)
la00noix 6:26724c287387 33 id1_value[4] = (msg.data[0]>>3)%4; //state of right joystick(1~3)
la00noix 6:26724c287387 34 id1_value[5] = (msg.data[0]>>2)%2; //left joystick neutral position(0 or 1)
la00noix 6:26724c287387 35 id1_value[6] = (msg.data[0]>>1)%2; //decide right/left(0 or 1)
la00noix 8:2ba338b4590e 36 t1_r = msg.data[3]; //value of t(0~7)
la00noix 8:2ba338b4590e 37
yuki0701 11:9db93bce4eef 38 // debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
yuki0701 11:9db93bce4eef 39 // ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);
yuki0701 11:9db93bce4eef 40 // debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]);
la00noix 0:c61c6e4775ca 41 }
yuki0701 3:8a0faa3b08c3 42
la00noix 4:317c53a674fa 43 if(msg.id == 3) {
yuki0701 11:9db93bce4eef 44 usw_flag = 0;
yuki0701 11:9db93bce4eef 45
yuki0701 1:3c11e07da92a 46 usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
la00noix 7:44ce34007499 47 //debug_printf("usw_data1 = %f\n\r",usw_data1);
yuki0701 3:8a0faa3b08c3 48
yuki0701 5:6cebe1c458a9 49 usw_data2 = 0.1 * (short)((msg.data[2]<<8) | msg.data[3]);
la00noix 7:44ce34007499 50 //debug_printf("usw_data2 = %f\n\r",usw_data2);
yuki0701 3:8a0faa3b08c3 51
yuki0701 5:6cebe1c458a9 52 usw_data3 = 0.1 * (short)((msg.data[4]<<8) | msg.data[5]);
la00noix 7:44ce34007499 53 //debug_printf("usw_data3 = %f\n\r",usw_data3);
yuki0701 3:8a0faa3b08c3 54
yuki0701 5:6cebe1c458a9 55 usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
la00noix 7:44ce34007499 56 //debug_printf("usw_data4 = %f\n\r",usw_data4);
la00noix 9:7667dcfc7ce5 57 //debug_printf("usw1=%f usw2=%f usw3=%f usw4=%f\n\r",usw_data1,usw_data2,usw_data3,usw_data4);
yuki0701 11:9db93bce4eef 58 }else{
yuki0701 11:9db93bce4eef 59 usw_flag = 1;
yuki0701 3:8a0faa3b08c3 60 }
yuki0701 3:8a0faa3b08c3 61
yuki0701 3:8a0faa3b08c3 62 } else {
la00noix 0:c61c6e4775ca 63 canread_led = 0;
la00noix 0:c61c6e4775ca 64 }
yuki0701 3:8a0faa3b08c3 65
la00noix 8:2ba338b4590e 66
la00noix 8:2ba338b4590e 67 can_ashileddata[1] = T1; //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
la00noix 8:2ba338b4590e 68
la00noix 8:2ba338b4590e 69 if(can1.write(CANMessage(4,can_ashileddata,2))) { //IDを4にして送信
yuki0701 11:9db93bce4eef 70 // printf("send\n\r");
la00noix 0:c61c6e4775ca 71 cansend_led = 1;
yuki0701 3:8a0faa3b08c3 72 } else {
yuki0701 11:9db93bce4eef 73 //printf("not_send\n\r");
la00noix 0:c61c6e4775ca 74 cansend_led = 0;
la00noix 0:c61c6e4775ca 75 }
yuki0701 3:8a0faa3b08c3 76
la00noix 8:2ba338b4590e 77 if(t1_r > T1) {
la00noix 8:2ba338b4590e 78 T1 = t1_r;
la00noix 8:2ba338b4590e 79 }
yuki0701 3:8a0faa3b08c3 80 }
la00noix 8:2ba338b4590e 81
yuki0701 3:8a0faa3b08c3 82 void can_start()
yuki0701 3:8a0faa3b08c3 83 {
yuki0701 3:8a0faa3b08c3 84
yuki0701 3:8a0faa3b08c3 85 while(1) {
yuki0701 3:8a0faa3b08c3 86
yuki0701 3:8a0faa3b08c3 87 CANMessage msg;
yuki0701 3:8a0faa3b08c3 88
yuki0701 3:8a0faa3b08c3 89 debug_printf("wait\n\r");
yuki0701 3:8a0faa3b08c3 90 wait(0.1);
yuki0701 3:8a0faa3b08c3 91 if(can1.read(msg)) {
yuki0701 3:8a0faa3b08c3 92 break;
yuki0701 3:8a0faa3b08c3 93 }
yuki0701 3:8a0faa3b08c3 94 }
la00noix 0:c61c6e4775ca 95 }
la00noix 0:c61c6e4775ca 96
yuki0701 3:8a0faa3b08c3 97 void UserLoopSetting_can()
yuki0701 3:8a0faa3b08c3 98 {
la00noix 0:c61c6e4775ca 99 can1.frequency(1000000);
la00noix 0:c61c6e4775ca 100 can_ticker.attach(&can_readsend,0.01); //遅かったら早める
la00noix 0:c61c6e4775ca 101 }