広田 勇斗
/
Omni_2017_z
Omni_2017_z
main.cpp@4:1326902ac7cd, 2017-08-05 (annotated)
- Committer:
- hirotayamato
- Date:
- Sat Aug 05 07:26:38 2017 +0000
- Revision:
- 4:1326902ac7cd
- Parent:
- 3:49450eb53b6f
Omni_2017_b
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hirotayamato | 3:49450eb53b6f | 1 | #include "2017_3_b.h" |
hirotayamato | 0:fd933ea5f19d | 2 | #include "mbed.h" |
hirotayamato | 0:fd933ea5f19d | 3 | #include <math.h> |
hirotayamato | 0:fd933ea5f19d | 4 | #include "MD_PID.h" |
hirotayamato | 0:fd933ea5f19d | 5 | #include "QEI.h" |
hirotayamato | 0:fd933ea5f19d | 6 | |
hirotayamato | 4:1326902ac7cd | 7 | Omni_3 Omni(p21, p18, p11, p12, p22, p17, p27, p28, p23, p16, p29, p30, 1); |
hirotayamato | 0:fd933ea5f19d | 8 | |
hirotayamato | 0:fd933ea5f19d | 9 | Serial pc (USBTX, USBRX); |
hirotayamato | 0:fd933ea5f19d | 10 | I2CSlave slave(p9, p10); |
hirotayamato | 0:fd933ea5f19d | 11 | |
hirotayamato | 0:fd933ea5f19d | 12 | DigitalOut check1(LED1); |
hirotayamato | 0:fd933ea5f19d | 13 | |
hirotayamato | 0:fd933ea5f19d | 14 | union Get_data{ |
hirotayamato | 0:fd933ea5f19d | 15 | char alldata[3]; |
hirotayamato | 0:fd933ea5f19d | 16 | struct{ |
hirotayamato | 0:fd933ea5f19d | 17 | signed int x :8; |
hirotayamato | 0:fd933ea5f19d | 18 | signed int y :8; |
hirotayamato | 0:fd933ea5f19d | 19 | signed int t :8; |
hirotayamato | 0:fd933ea5f19d | 20 | }data; |
hirotayamato | 0:fd933ea5f19d | 21 | }act; |
hirotayamato | 0:fd933ea5f19d | 22 | |
hirotayamato | 0:fd933ea5f19d | 23 | int main() { |
hirotayamato | 0:fd933ea5f19d | 24 | Omni.Drive(0, 0, 0); |
hirotayamato | 0:fd933ea5f19d | 25 | float x, y, st; |
hirotayamato | 0:fd933ea5f19d | 26 | |
hirotayamato | 0:fd933ea5f19d | 27 | slave.address(0x08); |
hirotayamato | 0:fd933ea5f19d | 28 | while(1) { |
hirotayamato | 0:fd933ea5f19d | 29 | int i = slave.receive(); |
hirotayamato | 0:fd933ea5f19d | 30 | check1 = 0.0; |
hirotayamato | 0:fd933ea5f19d | 31 | switch (i) { |
hirotayamato | 0:fd933ea5f19d | 32 | case I2CSlave::ReadAddressed: |
hirotayamato | 0:fd933ea5f19d | 33 | check1 = check1 ^ 1; |
hirotayamato | 0:fd933ea5f19d | 34 | break; |
hirotayamato | 0:fd933ea5f19d | 35 | case I2CSlave::WriteGeneral: |
hirotayamato | 0:fd933ea5f19d | 36 | slave.read(act.alldata, 3); |
hirotayamato | 0:fd933ea5f19d | 37 | check1 = check1 ^ 1; |
hirotayamato | 0:fd933ea5f19d | 38 | break; |
hirotayamato | 0:fd933ea5f19d | 39 | case I2CSlave::WriteAddressed: |
hirotayamato | 0:fd933ea5f19d | 40 | slave.read(act.alldata, 3); |
hirotayamato | 0:fd933ea5f19d | 41 | check1 = check1 ^ 1; |
hirotayamato | 0:fd933ea5f19d | 42 | x = (float)act.data.x / 127.0; |
hirotayamato | 0:fd933ea5f19d | 43 | y = (float)act.data.y / 127.0; |
hirotayamato | 0:fd933ea5f19d | 44 | st = (float)act.data.t / 127.0 * 0.1; |
hirotayamato | 0:fd933ea5f19d | 45 | break; |
hirotayamato | 0:fd933ea5f19d | 46 | } |
hirotayamato | 0:fd933ea5f19d | 47 | Omni.Drive(x, y, st); |
hirotayamato | 0:fd933ea5f19d | 48 | |
hirotayamato | 0:fd933ea5f19d | 49 | for(int i = 0; i < 3; i++) act.alldata[i] = 0; |
hirotayamato | 0:fd933ea5f19d | 50 | |
hirotayamato | 0:fd933ea5f19d | 51 | } |
hirotayamato | 0:fd933ea5f19d | 52 | } |
hirotayamato | 3:49450eb53b6f | 53 | |
hirotayamato | 4:1326902ac7cd | 54 |