春ロボ 2班 2017
主にオムニです。
Fork of
asimawari
by 
yuto kawamura
GET_position/jyairo.h@3:73625a6c9750, 2017-10-26 (annotated)
- Committer:
- yuto17320508
- Date:
- Thu Oct 26 13:10:28 2017 +0000
- Revision:
- 3:73625a6c9750
- Parent:
- 2:0a99389df632
a
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yuto17320508 | 3:73625a6c9750 | 1 | #include "RawSerial.h" |
| yuto17320508 | 3:73625a6c9750 | 2 | |
| yuto17320508 | 3:73625a6c9750 | 3 | Serial pc(USBTX, USBRX); // tx, rx |
| yuto17320508 | 3:73625a6c9750 | 4 | //Serial device(p13, p14); // tx, rx |
| yuto17320508 | 3:73625a6c9750 | 5 | RawSerial device(p13,p14); |
| yuto17320508 | 3:73625a6c9750 | 6 | AnalogIn tx(p19); |
| yuto17320508 | 3:73625a6c9750 | 7 | AnalogIn rx(p20); |
| yuto17320508 | 3:73625a6c9750 | 8 | #define CAN_ID 10 |
| yuto17320508 | 3:73625a6c9750 | 9 | CAN can1(p30, p29); |
| yuto17320508 | 3:73625a6c9750 | 10 | DigitalOut led1(LED1); //check |
| yuto17320508 | 3:73625a6c9750 | 11 | |
| yuto17320508 | 3:73625a6c9750 | 12 | Ticker ticker; |
| yuto17320508 | 3:73625a6c9750 | 13 | |
| yuto17320508 | 3:73625a6c9750 | 14 | uint8_t val[15]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; |
| yuto17320508 | 3:73625a6c9750 | 15 | uint8_t check_sum=0; |
| yuto17320508 | 3:73625a6c9750 | 16 | int16_t data_angle=0; |
| yuto17320508 | 3:73625a6c9750 | 17 | double angle=0; |
| yuto17320508 | 3:73625a6c9750 | 18 | bool protect=0; |
| yuto17320508 | 3:73625a6c9750 | 19 | |
| yuto17320508 | 3:73625a6c9750 | 20 | char send_data[8]={0,0,0,0,0,0,0,0}; |
| yuto17320508 | 3:73625a6c9750 | 21 | |
| yuto17320508 | 3:73625a6c9750 | 22 | void dev_rx () |
| yuto17320508 | 3:73625a6c9750 | 23 | { |
| yuto17320508 | 3:73625a6c9750 | 24 | val[0]=device.getc(); |
| yuto17320508 | 3:73625a6c9750 | 25 | val[1]=device.getc(); |
| yuto17320508 | 3:73625a6c9750 | 26 | if(val[0]==170 && val[1]==0) { |
| yuto17320508 | 3:73625a6c9750 | 27 | for(int i=2; i<14; i++) { |
| yuto17320508 | 3:73625a6c9750 | 28 | val[i]=device.getc(); |
| yuto17320508 | 3:73625a6c9750 | 29 | check_sum+=val[i]; |
| yuto17320508 | 3:73625a6c9750 | 30 | } |
| yuto17320508 | 3:73625a6c9750 | 31 | val[14]=device.getc(); |
| yuto17320508 | 3:73625a6c9750 | 32 | if(check_sum==val[14]) { |
| yuto17320508 | 3:73625a6c9750 | 33 | if(val[4]>0x80)data_angle=((val[3]&0xFF)|((val[4]<<8)&0xFF00))-0xFFFF; |
| yuto17320508 | 3:73625a6c9750 | 34 | else data_angle=((val[3]&0xFF)|((val[4]<<8)&0xFF00)); |
| yuto17320508 | 3:73625a6c9750 | 35 | angle=data_angle/100.0; |
| yuto17320508 | 3:73625a6c9750 | 36 | send_data[0]=data_angle>>8; |
| yuto17320508 | 3:73625a6c9750 | 37 | send_data[1]=data_angle&255; |
| yuto17320508 | 3:73625a6c9750 | 38 | if(can1.write(CANMessage(CAN_ID,send_data,2))) { |
| yuto17320508 | 3:73625a6c9750 | 39 | //pc.printf("send\r\n"); |
| yuto17320508 | 3:73625a6c9750 | 40 | } |
| yuto17320508 | 3:73625a6c9750 | 41 | check_sum=0; |
| yuto17320508 | 3:73625a6c9750 | 42 | } |
| yuto17320508 | 3:73625a6c9750 | 43 | } |
| yuto17320508 | 3:73625a6c9750 | 44 | } |
| yuto17320508 | 3:73625a6c9750 | 45 | |
| yuto17320508 | 3:73625a6c9750 | 46 | //setting |
| yuto17320508 | 3:73625a6c9750 | 47 | device.baud(115200); |
| yuto17320508 | 3:73625a6c9750 | 48 | device.format(8,Serial::None,1); |
| yuto17320508 | 3:73625a6c9750 | 49 | can1.frequency(1000000); |
| yuto17320508 | 3:73625a6c9750 | 50 | //ticker.attach(&dev_rx,0.05); |
| yuto17320508 | 3:73625a6c9750 | 51 | device.attach(dev_rx, Serial::RxIrq); |
| yuto17320508 | 3:73625a6c9750 | 52 | int a=0; |
| yuto17320508 | 3:73625a6c9750 | 53 |