harurobo_mbed_undercarriage_sub
Dependencies: mbed Maxon_setting move4wheel2 EC PathFollowing_12_22 CruizCore_R1370P
main.cpp@2:0073d971b618, 2018-12-22 (annotated)
- Committer:
- yuki0701
- Date:
- Sat Dec 22 02:55:31 2018 +0000
- Revision:
- 2:0073d971b618
- Parent:
- 1:0c9f53f5c9d0
a; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuki0701 | 0:f0f40dddc0c4 | 1 | #include "mbed.h" |
yuki0701 | 0:f0f40dddc0c4 | 2 | #include "EC.h" |
yuki0701 | 0:f0f40dddc0c4 | 3 | #include "R1370P.h" |
yuki0701 | 0:f0f40dddc0c4 | 4 | #include "move4wheel.h" |
yuki0701 | 0:f0f40dddc0c4 | 5 | #include "PathFollowing.h" |
yuki0701 | 1:0c9f53f5c9d0 | 6 | #include "Maxon_setting.h" |
yuki0701 | 0:f0f40dddc0c4 | 7 | |
yuki0701 | 1:0c9f53f5c9d0 | 8 | CAN can1(p30,p29,1000000); |
yuki0701 | 0:f0f40dddc0c4 | 9 | DigitalOut led(LED1); |
yuki0701 | 1:0c9f53f5c9d0 | 10 | Ticker ticker; |
yuki0701 | 0:f0f40dddc0c4 | 11 | |
yuki0701 | 0:f0f40dddc0c4 | 12 | void can_read(){//CAN通信受信 |
yuki0701 | 0:f0f40dddc0c4 | 13 | |
yuki0701 | 0:f0f40dddc0c4 | 14 | CANMessage msg; |
yuki0701 | 0:f0f40dddc0c4 | 15 | |
yuki0701 | 0:f0f40dddc0c4 | 16 | if(can1.read(msg)){ |
yuki0701 | 0:f0f40dddc0c4 | 17 | |
yuki0701 | 0:f0f40dddc0c4 | 18 | if(msg.id == 1){ |
yuki0701 | 0:f0f40dddc0c4 | 19 | led = 1; |
yuki0701 | 1:0c9f53f5c9d0 | 20 | usw_data1 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); |
yuki0701 | 1:0c9f53f5c9d0 | 21 | printf("usw_data1 = %d:%d,%lf\n\r",msg.data[0],msg.data[1],usw_data1); |
yuki0701 | 0:f0f40dddc0c4 | 22 | }else if(msg.id == 2){ |
yuki0701 | 1:0c9f53f5c9d0 | 23 | //led = 1; |
yuki0701 | 1:0c9f53f5c9d0 | 24 | // usw_data2 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); |
yuki0701 | 1:0c9f53f5c9d0 | 25 | //printf("usw_data2 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data2); |
yuki0701 | 0:f0f40dddc0c4 | 26 | }else if(msg.id == 3){ |
yuki0701 | 1:0c9f53f5c9d0 | 27 | //led = 1; |
yuki0701 | 1:0c9f53f5c9d0 | 28 | //usw_data3 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); |
yuki0701 | 1:0c9f53f5c9d0 | 29 | //printf("usw_data3 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data3); |
yuki0701 | 0:f0f40dddc0c4 | 30 | }else if(msg.id == 4){ |
yuki0701 | 1:0c9f53f5c9d0 | 31 | //led = 1; |
yuki0701 | 1:0c9f53f5c9d0 | 32 | //usw_data4 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); |
yuki0701 | 1:0c9f53f5c9d0 | 33 | //printf("usw_data4 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data4); |
yuki0701 | 0:f0f40dddc0c4 | 34 | } |
yuki0701 | 0:f0f40dddc0c4 | 35 | |
yuki0701 | 0:f0f40dddc0c4 | 36 | } |
yuki0701 | 0:f0f40dddc0c4 | 37 | } |
yuki0701 | 0:f0f40dddc0c4 | 38 | |
yuki0701 | 0:f0f40dddc0c4 | 39 | |
yuki0701 | 0:f0f40dddc0c4 | 40 | //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// |
yuki0701 | 0:f0f40dddc0c4 | 41 | int main() |
yuki0701 | 1:0c9f53f5c9d0 | 42 | { |
yuki0701 | 0:f0f40dddc0c4 | 43 | UserLoopSetting(); |
yuki0701 | 1:0c9f53f5c9d0 | 44 | //UserLoopSetting2(); |
yuki0701 | 0:f0f40dddc0c4 | 45 | //Debug_Control() |
yuki0701 | 0:f0f40dddc0c4 | 46 | |
yuki0701 | 0:f0f40dddc0c4 | 47 | |
yuki0701 | 1:0c9f53f5c9d0 | 48 | //purecurve2(5,1,1,0,0,-500,-500,9,1500,5,0.1,10,0.1,600,0); |
yuki0701 | 1:0c9f53f5c9d0 | 49 | //gogo_straight(1,1,0,0,500,0,2000,2000,5,0.1,10,0.1,600,0); |
yuki0701 | 1:0c9f53f5c9d0 | 50 | |
yuki0701 | 1:0c9f53f5c9d0 | 51 | ticker.attach(&can_read,0.01); |
yuki0701 | 1:0c9f53f5c9d0 | 52 | |
yuki0701 | 1:0c9f53f5c9d0 | 53 | while(1); |
yuki0701 | 1:0c9f53f5c9d0 | 54 | //printf("%f\n\r",usw_data1); |
yuki0701 | 1:0c9f53f5c9d0 | 55 | |
yuki0701 | 0:f0f40dddc0c4 | 56 | |
yuki0701 | 1:0c9f53f5c9d0 | 57 | //MotorControl(0,0,0,0); |
yuki0701 | 1:0c9f53f5c9d0 | 58 | } |