ロボステ6期 / Mbed 2 deprecated NHK2020-main１

あ

Dependencies:   uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

can/can.cpp@2:820dcd23c8e3, 2019-12-11 (annotated)

Committer:
yuki0701
Date:
Wed Dec 11 04:59:36 2019 +0000
Revision:
2:820dcd23c8e3
Parent:
1:26fc1b2f1c42 
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:b87fd8dd4322 1 #include "mbed.h"
la00noix 0:b87fd8dd4322 2 #include "PathFollowing.h"
la00noix 0:b87fd8dd4322 3 #include "movement.h"
la00noix 0:b87fd8dd4322 4 #include "manual.h"
la00noix 0:b87fd8dd4322 5 #include "can.h"
la00noix 0:b87fd8dd4322 6
la00noix 0:b87fd8dd4322 7 CAN can1(p30,p29);
la00noix 0:b87fd8dd4322 8 Ticker can_ticker; //can用ticker
la00noix 0:b87fd8dd4322 9
la00noix 0:b87fd8dd4322 10 DigitalOut cansend_led(LED1); //cansend -> on
la00noix 0:b87fd8dd4322 11 DigitalOut canread_led(LED2); //canread -> on
la00noix 0:b87fd8dd4322 12
la00noix 0:b87fd8dd4322 13 int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
la00noix 0:b87fd8dd4322 14
la00noix 0:b87fd8dd4322 15 void can_readsend()
la00noix 0:b87fd8dd4322 16 {
la00noix 0:b87fd8dd4322 17 CANMessage msg;
la00noix 0:b87fd8dd4322 18
yuki0701 1:26fc1b2f1c42 19 /*
la00noix 0:b87fd8dd4322 20 if(can1.read(msg)) {
la00noix 0:b87fd8dd4322 21
la00noix 0:b87fd8dd4322 22 if(msg.id == 1) { //from main
la00noix 0:b87fd8dd4322 23
la00noix 0:b87fd8dd4322 24 id1_value[0] = (msg.data[0]>>6)%4; //decide wait/auto/manual(0~2)
la00noix 0:b87fd8dd4322 25 id1_value[1] = msg.data[1]; //angle of left joystick(0~359)
la00noix 0:b87fd8dd4322 26 id1_value[2] = msg.data[2];
la00noix 0:b87fd8dd4322 27 id1_value[3] = (msg.data[0]>>5)%2; //BOTTONR1 off/on(0 or 1)
la00noix 0:b87fd8dd4322 28 id1_value[4] = (msg.data[0]>>3)%4; //state of right joystick(1~3)
la00noix 0:b87fd8dd4322 29 id1_value[5] = (msg.data[0]>>2)%2; //left joystick neutral position(0 or 1)
la00noix 0:b87fd8dd4322 30 id1_value[6] = (msg.data[0]>>1)%2; //decide right/left(0 or 1)
yuki0701 1:26fc1b2f1c42 31 t1_r = msg.data[3]; //value of t(0~7)*/
la00noix 0:b87fd8dd4322 32
yuki0701 1:26fc1b2f1c42 33 /*debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
yuki0701 1:26fc1b2f1c42 34 ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);*/
yuki0701 1:26fc1b2f1c42 35 /* debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]);
yuki0701 1:26fc1b2f1c42 36 }
yuki0701 1:26fc1b2f1c42 37
yuki0701 1:26fc1b2f1c42 38 if(msg.id == 3) {
yuki0701 1:26fc1b2f1c42 39 usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
yuki0701 1:26fc1b2f1c42 40 //debug_printf("usw_data1 = %f\n\r",usw_data1);
la00noix 0:b87fd8dd4322 41
yuki0701 1:26fc1b2f1c42 42 usw_data2 = 0.1 * (short)((msg.data[2]<<8) | msg.data[3]);
yuki0701 1:26fc1b2f1c42 43 //debug_printf("usw_data2 = %f\n\r",usw_data2);
la00noix 0:b87fd8dd4322 44
yuki0701 1:26fc1b2f1c42 45 usw_data3 = 0.1 * (short)((msg.data[4]<<8) | msg.data[5]);
yuki0701 1:26fc1b2f1c42 46 //debug_printf("usw_data3 = %f\n\r",usw_data3);
la00noix 0:b87fd8dd4322 47
yuki0701 1:26fc1b2f1c42 48 usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
yuki0701 1:26fc1b2f1c42 49 //debug_printf("usw_data4 = %f\n\r",usw_data4);
yuki0701 1:26fc1b2f1c42 50 //debug_printf("usw1=%f usw2=%f usw3=%f usw4=%f\n\r",usw_data1,usw_data2,usw_data3,usw_data4);
yuki0701 1:26fc1b2f1c42 51 canread_led = 1;
yuki0701 1:26fc1b2f1c42 52 }
la00noix 0:b87fd8dd4322 53
yuki0701 1:26fc1b2f1c42 54 } else {
yuki0701 1:26fc1b2f1c42 55 canread_led = 0;
yuki0701 1:26fc1b2f1c42 56 }*/
la00noix 0:b87fd8dd4322 57
la00noix 0:b87fd8dd4322 58
la00noix 0:b87fd8dd4322 59 can_ashileddata[1] = T1; //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
la00noix 0:b87fd8dd4322 60
yuki0701 1:26fc1b2f1c42 61 can_ashileddata2[0] = m1 >> 8;
yuki0701 1:26fc1b2f1c42 62 can_ashileddata2[1] = m1 &255;
yuki0701 1:26fc1b2f1c42 63
yuki0701 1:26fc1b2f1c42 64 can_ashileddata2[2] = m2 >> 8;
yuki0701 1:26fc1b2f1c42 65 can_ashileddata2[3] = m2 &255;
yuki0701 1:26fc1b2f1c42 66
yuki0701 1:26fc1b2f1c42 67 can_ashileddata2[4] = m3 >> 8;
yuki0701 1:26fc1b2f1c42 68 can_ashileddata2[5] = m3 &255;
yuki0701 1:26fc1b2f1c42 69
yuki0701 1:26fc1b2f1c42 70 can_ashileddata2[6] = m4 >> 8;
yuki0701 1:26fc1b2f1c42 71 can_ashileddata2[7] = m4 &255;
yuki0701 1:26fc1b2f1c42 72
yuki0701 1:26fc1b2f1c42 73
yuki0701 1:26fc1b2f1c42 74 // can_ashileddata3[0] = m2 >> 8;
yuki0701 1:26fc1b2f1c42 75 // can_ashileddata3[1] = m2 &255;
yuki0701 1:26fc1b2f1c42 76 //
yuki0701 1:26fc1b2f1c42 77 // can_ashileddata4[0] = m3 >> 8;
yuki0701 1:26fc1b2f1c42 78 // can_ashileddata4[1] = m3 &255;
yuki0701 1:26fc1b2f1c42 79 //
yuki0701 1:26fc1b2f1c42 80 // can_ashileddata5[0] = m4 >> 8;
yuki0701 1:26fc1b2f1c42 81 // can_ashileddata5[1] = m4 &255;
yuki0701 1:26fc1b2f1c42 82
yuki0701 1:26fc1b2f1c42 83 if(can1.write(CANMessage(4,can_ashileddata2,8))) { //IDを4にして送信
yuki0701 1:26fc1b2f1c42 84 //printf("success : %d \n\r",m1);
la00noix 0:b87fd8dd4322 85 cansend_led = 1;
la00noix 0:b87fd8dd4322 86 } else {
la00noix 0:b87fd8dd4322 87 cansend_led = 0;
yuki0701 1:26fc1b2f1c42 88 //printf("fale\n\r");
la00noix 0:b87fd8dd4322 89 }
la00noix 0:b87fd8dd4322 90
yuki0701 1:26fc1b2f1c42 91 //if(can1.write(CANMessage(5,can_ashileddata3,2))) { //IDを4にして送信
yuki0701 1:26fc1b2f1c42 92 // cansend_led = 1;
yuki0701 1:26fc1b2f1c42 93 // } else {
yuki0701 1:26fc1b2f1c42 94 // cansend_led = 0;
yuki0701 1:26fc1b2f1c42 95 // }
yuki0701 1:26fc1b2f1c42 96 //
yuki0701 1:26fc1b2f1c42 97 // if(can1.write(CANMessage(6,can_ashileddata4,2))) { //IDを4にして送信
yuki0701 1:26fc1b2f1c42 98 // cansend_led = 1;
yuki0701 1:26fc1b2f1c42 99 // } else {
yuki0701 1:26fc1b2f1c42 100 // cansend_led = 0;
yuki0701 1:26fc1b2f1c42 101 // }
yuki0701 1:26fc1b2f1c42 102 //
yuki0701 1:26fc1b2f1c42 103 // if(can1.write(CANMessage(7,can_ashileddata5,2))) { //IDを4にして送信
yuki0701 1:26fc1b2f1c42 104 // cansend_led = 1;
yuki0701 1:26fc1b2f1c42 105 // } else {
yuki0701 1:26fc1b2f1c42 106 // cansend_led = 0;
yuki0701 1:26fc1b2f1c42 107 // }
yuki0701 1:26fc1b2f1c42 108
yuki0701 1:26fc1b2f1c42 109 /*if(t1_r > T1) {
la00noix 0:b87fd8dd4322 110 T1 = t1_r;
yuki0701 1:26fc1b2f1c42 111 }*/
la00noix 0:b87fd8dd4322 112 }
la00noix 0:b87fd8dd4322 113
la00noix 0:b87fd8dd4322 114 void can_start()
la00noix 0:b87fd8dd4322 115 {
la00noix 0:b87fd8dd4322 116
la00noix 0:b87fd8dd4322 117 while(1) {
la00noix 0:b87fd8dd4322 118
la00noix 0:b87fd8dd4322 119 CANMessage msg;
la00noix 0:b87fd8dd4322 120
yuki0701 1:26fc1b2f1c42 121 // debug_printf("wait\n\r");
yuki0701 1:26fc1b2f1c42 122 printf("wait\n\r");
la00noix 0:b87fd8dd4322 123 wait(0.1);
la00noix 0:b87fd8dd4322 124 if(can1.read(msg)) {
la00noix 0:b87fd8dd4322 125 break;
la00noix 0:b87fd8dd4322 126 }
la00noix 0:b87fd8dd4322 127 }
la00noix 0:b87fd8dd4322 128 }
la00noix 0:b87fd8dd4322 129
la00noix 0:b87fd8dd4322 130 void UserLoopSetting_can()
la00noix 0:b87fd8dd4322 131 {
la00noix 0:b87fd8dd4322 132 can1.frequency(1000000);
la00noix 0:b87fd8dd4322 133 can_ticker.attach(&can_readsend,0.01); //遅かったら早める
la00noix 0:b87fd8dd4322 134 }