NHK2017Ateamかにろぼ
Dependencies: 2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ //\| |/\\ //\| |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/controller/controller.cpp@9:cba664555161, 2017-08-29 (annotated)
- Committer:
- uchitake
- Date:
- Tue Aug 29 13:46:09 2017 +0900
- Revision:
- 9:cba664555161
- Parent:
- 5:bc5ba4f070ad
- Child:
- 15:9a2dce34b660
fix pinName
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 |
9:cba664555161 | 3 | Controller::Controller() : FEP(XBee1TX, XBee1RX, ADDR) |
UCHITAKE | 0:757e9800c103 | 4 | { |
UCHITAKE | 1:269914e0aa07 | 5 | for(int i = 0; i < 7; i++) Button1[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 6 | for(int i = 0; i < 6; i++) Button2[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 7 | for(int i = 0; i < 6; i++) data[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 8 | for(int i = 0; i < 4; i++) stick[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 9 | for(int i = 0; i < 2; i++) degree[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 10 | for(int i = 0; i < 2; i++) norm[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 11 | for(int i = 0; i < 2; i++) vector[i] = 0; |
UCHITAKE | 1:269914e0aa07 | 12 | fepTemp = 0; |
UCHITAKE | 1:269914e0aa07 | 13 | } |
UCHITAKE | 1:269914e0aa07 | 14 | |
UCHITAKE | 1:269914e0aa07 | 15 | void Controller::receiveState() |
UCHITAKE | 1:269914e0aa07 | 16 | { |
UCHITAKE | 1:269914e0aa07 | 17 | fepTemp = read(data, 6); |
UCHITAKE | 1:269914e0aa07 | 18 | if(fepTemp == FEP_SUCCESS) { |
UCHITAKE | 1:269914e0aa07 | 19 | for(int i=0; i<7; i++) { |
UCHITAKE | 1:269914e0aa07 | 20 | Button1[i] = data[4] % 2; |
UCHITAKE | 1:269914e0aa07 | 21 | data[4] /= 2; |
UCHITAKE | 1:269914e0aa07 | 22 | } |
UCHITAKE | 1:269914e0aa07 | 23 | for(int i=0; i<6; i++) { |
UCHITAKE | 1:269914e0aa07 | 24 | Button2[i] = data[5] % 2; |
UCHITAKE | 1:269914e0aa07 | 25 | data[5] /= 2; |
UCHITAKE | 1:269914e0aa07 | 26 | } |
UCHITAKE | 1:269914e0aa07 | 27 | for(int i = 0; i < 4; i++) { |
UCHITAKE | 1:269914e0aa07 | 28 | stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0); |
UCHITAKE | 1:269914e0aa07 | 29 | } |
UCHITAKE | 1:269914e0aa07 | 30 | getStickData(); |
UCHITAKE | 1:269914e0aa07 | 31 | } else if(fepTemp == FEP_NO_RESPONSE) { |
UCHITAKE | 1:269914e0aa07 | 32 | } else { |
UCHITAKE | 1:269914e0aa07 | 33 | } |
UCHITAKE | 1:269914e0aa07 | 34 | } |
UCHITAKE | 1:269914e0aa07 | 35 | |
UCHITAKE | 1:269914e0aa07 | 36 | void Controller::getStickData() |
UCHITAKE | 1:269914e0aa07 | 37 | { |
UCHITAKE | 5:bc5ba4f070ad | 38 | for(int i = 0; i < 4; i++) { |
UCHITAKE | 5:bc5ba4f070ad | 39 | if(stick[i] < 0.1 && stick[i] > -0.1) stick[i] = 0; |
UCHITAKE | 5:bc5ba4f070ad | 40 | } |
UCHITAKE | 1:269914e0aa07 | 41 | degree[0] = (atan2(stick[1], -stick[0])) * (180 / M_PI); |
UCHITAKE | 1:269914e0aa07 | 42 | degree[1] = (atan2(stick[3], -stick[2])) * (180 / M_PI); |
UCHITAKE | 1:269914e0aa07 | 43 | |
UCHITAKE | 1:269914e0aa07 | 44 | norm[0] = hypot(stick[0], stick[1]); |
UCHITAKE | 1:269914e0aa07 | 45 | norm[1] = hypot(stick[2], stick[3]); |
UCHITAKE | 1:269914e0aa07 | 46 | |
UCHITAKE | 1:269914e0aa07 | 47 | if(norm[0] < STICK_NEWTRAL) norm[0] = 0; |
UCHITAKE | 1:269914e0aa07 | 48 | if(norm[1] < STICK_NEWTRAL) norm[1] = 0; |
UCHITAKE | 1:269914e0aa07 | 49 | if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX; |
UCHITAKE | 1:269914e0aa07 | 50 | if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX; |
UCHITAKE | 1:269914e0aa07 | 51 | |
UCHITAKE | 1:269914e0aa07 | 52 | vector[0] = degree[0]; |
UCHITAKE | 1:269914e0aa07 | 53 | vector[1] = norm[0]; |
UCHITAKE | 2:ea151e05033a | 54 | vector2[0] = degree[1]; |
UCHITAKE | 2:ea151e05033a | 55 | vector2[1] = norm[1]; |
uchitake |
9:cba664555161 | 56 | } |