NHK2017Ateamかにろぼ
Dependencies: 2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ //\| |/\\ //\| |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/controller/controller.cpp
- Committer:
- UCHITAKE
- Date:
- 2017-09-04
- Revision:
- 19:3a62cbc6fee9
- Parent:
- 16:86958857c739
- Child:
- 22:bb6afe7332c3
File content as of revision 19:3a62cbc6fee9:
#include "controller.h" Controller::Controller() : FEP(XBee1TX, XBee1RX, ADDR) { for(int i = 0; i < 7; i++) button1[i] = 0; for(int i = 0; i < 6; i++) button2[i] = 0; for(int i = 0; i < 6; i++) data[i] = 0; for(int i = 0; i < 4; i++) stick[i] = 0; for(int i = 0; i < 2; i++) radian[i] = 0; for(int i = 0; i < 2; i++) norm[i] = 0; fepTemp = 0; } Controller::Controller(PinName FEPtx, PinName FEPrx, int addr) : FEP(FEPtx, FEPrx, addr) { for(int i = 0; i < 7; i++) button1[i] = 0; for(int i = 0; i < 6; i++) button2[i] = 0; for(int i = 0; i < 6; i++) data[i] = 0; for(int i = 0; i < 4; i++) stick[i] = 0; for(int i = 0; i < 2; i++) radian[i] = 0; for(int i = 0; i < 2; i++) norm[i] = 0; fepTemp = 0; } bool Controller::receiveState() { fepTemp = FEP::read(data, DATA_SIZE); if(fepTemp == FEP_SUCCESS) { for(int i = 0; i < 7; i++) { button1[i] = data[4] % 2; data[4] /= 2; } for(int i=0; i<6; i++) { button2[i] = data[5] % 2; data[5] /= 2; } for(int i = 0; i < 4; i++) { stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0); } setStick(); } else if(fepTemp == FEP_NO_RESPONSE) { return 0; } else { return 0; } return 1; } void Controller::setStick() { for(int i = 0; i < 4; i++) { if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0; } radian[0] = atan2(stick[1], -stick[0]); radian[1] = atan2(stick[3], -stick[2]); norm[0] = hypot(stick[0], stick[1]); norm[1] = hypot(stick[2], stick[3]); if(norm[0] < STICK_NEWTRAL) norm[0] = 0; if(norm[1] < STICK_NEWTRAL) norm[1] = 0; if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX; if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX; } bool Controller::getButton1(int number) { return button1[number]; } bool Controller::getButton2(int number) { return button2[number]; } float Controller::getStick(int number) { return stick[number]; } float Controller::getRadian(int number) { return radian[number]; } float Controller::getNorm(int number) { return norm[number]; }