NHK2017Ateamかにろぼ
Dependencies: 2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ //\| |/\\ //\| |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/controller/controller.cpp@22:bb6afe7332c3, 2017-09-05 (annotated)
- Committer:
- uchitake
- Date:
- Tue Sep 05 12:00:34 2017 +0900
- Revision:
- 22:bb6afe7332c3
- Parent:
- 19:3a62cbc6fee9
fix Security!!!!!!!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:757e9800c103 | 1 | #include "controller.h" |
UCHITAKE | 0:757e9800c103 | 2 | |
uchitake |
22:bb6afe7332c3 | 3 | Controller::Controller() : |
uchitake |
22:bb6afe7332c3 | 4 | FEP(XBee1TX, XBee1RX, ADDR), |
uchitake |
22:bb6afe7332c3 | 5 | data(), |
uchitake |
22:bb6afe7332c3 | 6 | fepTemp(0), |
uchitake |
22:bb6afe7332c3 | 7 | button1(), |
uchitake |
22:bb6afe7332c3 | 8 | button2(), |
uchitake |
22:bb6afe7332c3 | 9 | stick(), |
uchitake |
22:bb6afe7332c3 | 10 | radian(), |
uchitake |
22:bb6afe7332c3 | 11 | norm() |
UCHITAKE | 0:757e9800c103 | 12 | { |
UCHITAKE | 1:269914e0aa07 | 13 | } |
UCHITAKE | 1:269914e0aa07 | 14 | |
uchitake |
16:86958857c739 | 15 | Controller::Controller(PinName FEPtx, PinName FEPrx, int addr) : |
uchitake |
22:bb6afe7332c3 | 16 | FEP(FEPtx, FEPrx, addr), |
uchitake |
22:bb6afe7332c3 | 17 | data(), |
uchitake |
22:bb6afe7332c3 | 18 | fepTemp(0), |
uchitake |
22:bb6afe7332c3 | 19 | button1(), |
uchitake |
22:bb6afe7332c3 | 20 | button2(), |
uchitake |
22:bb6afe7332c3 | 21 | stick(), |
uchitake |
22:bb6afe7332c3 | 22 | radian(), |
uchitake |
22:bb6afe7332c3 | 23 | norm() |
uchitake |
16:86958857c739 | 24 | { |
uchitake |
16:86958857c739 | 25 | } |
uchitake |
16:86958857c739 | 26 | |
UCHITAKE | 19:3a62cbc6fee9 | 27 | bool Controller::receiveState() |
UCHITAKE | 1:269914e0aa07 | 28 | { |
uchitake |
15:9a2dce34b660 | 29 | fepTemp = FEP::read(data, DATA_SIZE); |
UCHITAKE | 1:269914e0aa07 | 30 | if(fepTemp == FEP_SUCCESS) { |
uchitake |
15:9a2dce34b660 | 31 | for(int i = 0; i < 7; i++) { |
uchitake |
15:9a2dce34b660 | 32 | button1[i] = data[4] % 2; |
UCHITAKE | 1:269914e0aa07 | 33 | data[4] /= 2; |
UCHITAKE | 1:269914e0aa07 | 34 | } |
UCHITAKE | 1:269914e0aa07 | 35 | for(int i=0; i<6; i++) { |
uchitake |
15:9a2dce34b660 | 36 | button2[i] = data[5] % 2; |
UCHITAKE | 1:269914e0aa07 | 37 | data[5] /= 2; |
UCHITAKE | 1:269914e0aa07 | 38 | } |
UCHITAKE | 1:269914e0aa07 | 39 | for(int i = 0; i < 4; i++) { |
UCHITAKE | 1:269914e0aa07 | 40 | stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0); |
UCHITAKE | 1:269914e0aa07 | 41 | } |
uchitake |
15:9a2dce34b660 | 42 | setStick(); |
UCHITAKE | 1:269914e0aa07 | 43 | } else if(fepTemp == FEP_NO_RESPONSE) { |
UCHITAKE | 19:3a62cbc6fee9 | 44 | return 0; |
UCHITAKE | 1:269914e0aa07 | 45 | } else { |
UCHITAKE | 19:3a62cbc6fee9 | 46 | return 0; |
UCHITAKE | 1:269914e0aa07 | 47 | } |
UCHITAKE | 19:3a62cbc6fee9 | 48 | return 1; |
UCHITAKE | 1:269914e0aa07 | 49 | } |
UCHITAKE | 1:269914e0aa07 | 50 | |
uchitake |
15:9a2dce34b660 | 51 | void Controller::setStick() |
UCHITAKE | 1:269914e0aa07 | 52 | { |
UCHITAKE | 5:bc5ba4f070ad | 53 | for(int i = 0; i < 4; i++) { |
uchitake |
15:9a2dce34b660 | 54 | if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0; |
UCHITAKE | 5:bc5ba4f070ad | 55 | } |
uchitake |
15:9a2dce34b660 | 56 | |
uchitake |
15:9a2dce34b660 | 57 | radian[0] = atan2(stick[1], -stick[0]); |
uchitake |
15:9a2dce34b660 | 58 | radian[1] = atan2(stick[3], -stick[2]); |
UCHITAKE | 1:269914e0aa07 | 59 | |
UCHITAKE | 1:269914e0aa07 | 60 | norm[0] = hypot(stick[0], stick[1]); |
UCHITAKE | 1:269914e0aa07 | 61 | norm[1] = hypot(stick[2], stick[3]); |
UCHITAKE | 1:269914e0aa07 | 62 | |
UCHITAKE | 1:269914e0aa07 | 63 | if(norm[0] < STICK_NEWTRAL) norm[0] = 0; |
UCHITAKE | 1:269914e0aa07 | 64 | if(norm[1] < STICK_NEWTRAL) norm[1] = 0; |
UCHITAKE | 1:269914e0aa07 | 65 | if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX; |
UCHITAKE | 1:269914e0aa07 | 66 | if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX; |
uchitake |
9:cba664555161 | 67 | } |
uchitake |
16:86958857c739 | 68 | |
uchitake |
22:bb6afe7332c3 | 69 | bool Controller::getButton1(int number) const |
uchitake |
16:86958857c739 | 70 | { |
uchitake |
16:86958857c739 | 71 | return button1[number]; |
uchitake |
16:86958857c739 | 72 | } |
uchitake |
16:86958857c739 | 73 | |
uchitake |
22:bb6afe7332c3 | 74 | bool Controller::getButton2(int number) const |
uchitake |
16:86958857c739 | 75 | { |
uchitake |
16:86958857c739 | 76 | return button2[number]; |
uchitake |
16:86958857c739 | 77 | } |
uchitake |
16:86958857c739 | 78 | |
uchitake |
22:bb6afe7332c3 | 79 | float Controller::getStick(int number) const |
uchitake |
16:86958857c739 | 80 | { |
uchitake |
16:86958857c739 | 81 | return stick[number]; |
uchitake |
16:86958857c739 | 82 | } |
uchitake |
16:86958857c739 | 83 | |
uchitake |
22:bb6afe7332c3 | 84 | float Controller::getRadian(int number) const |
uchitake |
16:86958857c739 | 85 | { |
uchitake |
16:86958857c739 | 86 | return radian[number]; |
uchitake |
16:86958857c739 | 87 | } |
uchitake |
16:86958857c739 | 88 | |
uchitake |
22:bb6afe7332c3 | 89 | float Controller::getNorm(int number) const |
uchitake |
16:86958857c739 | 90 | { |
uchitake |
16:86958857c739 | 91 | return norm[number]; |
uchitake |
16:86958857c739 | 92 | } |