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

can出来ない～

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

can/can.cpp@10:6d60c0927770, 2019-03-06 (annotated)

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