R1370
Dependents: NHK2017_octopus2 NHK2017_octopus2_drive 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
Fork of R1307 by
r1307.cpp@2:deb20a97e035, 2017-10-11 (annotated)
- Committer:
- WAT34
- Date:
- Wed Oct 11 18:00:52 2017 +0900
- Revision:
- 2:deb20a97e035
- Parent:
- 1:59efa65938c7
- Child:
- 3:7105b2e1f51e
bugs fixed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 1:59efa65938c7 | 1 | #include "r1307.h" |
WAT34 | 1:59efa65938c7 | 2 | |
WAT34 | 1:59efa65938c7 | 3 | |
WAT34 | 1:59efa65938c7 | 4 | R1307::R1307(PinName tx_,PinName rx_): serial(tx_,rx_) |
WAT34 | 1:59efa65938c7 | 5 | { |
WAT34 | 2:deb20a97e035 | 6 | //ticker.attach(this,&R1307::update,0.01); |
WAT34 | 1:59efa65938c7 | 7 | serial.baud(115200); |
WAT34 | 1:59efa65938c7 | 8 | } |
WAT34 | 1:59efa65938c7 | 9 | |
WAT34 | 1:59efa65938c7 | 10 | |
WAT34 | 1:59efa65938c7 | 11 | void R1307::update() |
WAT34 | 1:59efa65938c7 | 12 | { |
WAT34 | 1:59efa65938c7 | 13 | uint8_t index; |
WAT34 | 1:59efa65938c7 | 14 | int16_t angle; |
WAT34 | 1:59efa65938c7 | 15 | int16_t rate; |
WAT34 | 1:59efa65938c7 | 16 | int16_t x_acc; |
WAT34 | 1:59efa65938c7 | 17 | int16_t y_acc; |
WAT34 | 1:59efa65938c7 | 18 | int16_t z_acc; |
WAT34 | 1:59efa65938c7 | 19 | uint8_t check_sum; |
WAT34 | 1:59efa65938c7 | 20 | //Verify packet heading information |
WAT34 | 1:59efa65938c7 | 21 | while(serial.getc() != 0xAA){ |
WAT34 | 1:59efa65938c7 | 22 | if(!serial.readable()) |
WAT34 | 1:59efa65938c7 | 23 | return; |
WAT34 | 1:59efa65938c7 | 24 | } |
WAT34 | 1:59efa65938c7 | 25 | if(serial.getc() != 0x00) |
WAT34 | 1:59efa65938c7 | 26 | return; |
WAT34 | 1:59efa65938c7 | 27 | //Assemble data |
WAT34 | 1:59efa65938c7 | 28 | |
WAT34 | 1:59efa65938c7 | 29 | index = serial.getc(); |
WAT34 | 1:59efa65938c7 | 30 | rate = (serial.getc() & 0xFF) | ((serial.getc() << 8) & 0xFF00); |
WAT34 | 1:59efa65938c7 | 31 | angle = (serial.getc() & 0xFF) | ((serial.getc()<< 8) & 0XFF00); |
WAT34 | 1:59efa65938c7 | 32 | x_acc = (serial.getc() & 0xFF) | ((serial.getc()<< 8) & 0xFF00); |
WAT34 | 1:59efa65938c7 | 33 | y_acc = (serial.getc() & 0xFF) | ((serial.getc() << 8) & 0XFF00); |
WAT34 | 1:59efa65938c7 | 34 | z_acc = (serial.getc() & 0xFF) | ((serial.getc() << 8) & 0xFF00); |
WAT34 | 1:59efa65938c7 | 35 | //Scale and store data |
WAT34 | 1:59efa65938c7 | 36 | gRate = rate / 100.0; |
WAT34 | 1:59efa65938c7 | 37 | gAngle = angle / 100.0; |
WAT34 | 1:59efa65938c7 | 38 | gX_acc = x_acc; |
WAT34 | 1:59efa65938c7 | 39 | gY_acc = y_acc; |
WAT34 | 1:59efa65938c7 | 40 | gZ_acc = z_acc; |
WAT34 | 1:59efa65938c7 | 41 | return; |
WAT34 | 1:59efa65938c7 | 42 | } |