NagaokaRoboticsClub_mbedTeam / Controller

交流ロボコン

Dependencies:   FEP

Dependents:   kourobo_Controller_FEP199_198 kourobo_controller_FEP209_208 kourobo2019 kouroboA_2019 ... more

controller.cpp@0:875532e626ee, 2018-01-15 (annotated)

Committer:
number_key
Date:
Mon Jan 15 09:21:59 2018 +0000
Revision:
0:875532e626ee
compactControllerReciever

Who changed what in which revision?

UserRevisionLine numberNew contents of line
number_key 0:875532e626ee 1 #include "controller.h"
number_key 0:875532e626ee 2
number_key 0:875532e626ee 3 Controller::Controller(PinName FEPtx, PinName FEPrx, int addr) :
number_key 0:875532e626ee 4 FEP(FEPtx, FEPrx, addr),
number_key 0:875532e626ee 5 data(),
number_key 0:875532e626ee 6 fepTemp(0),
number_key 0:875532e626ee 7 button1(),
number_key 0:875532e626ee 8 button2(),
number_key 0:875532e626ee 9 stick(),
number_key 0:875532e626ee 10 radian(),
number_key 0:875532e626ee 11 norm()
number_key 0:875532e626ee 12 {
number_key 0:875532e626ee 13 }
number_key 0:875532e626ee 14
number_key 0:875532e626ee 15 bool Controller::receiveState()
number_key 0:875532e626ee 16 {
number_key 0:875532e626ee 17 fepTemp = FEP::read(data, DATA_SIZE);
number_key 0:875532e626ee 18 if(fepTemp == FEP_SUCCESS) {
number_key 0:875532e626ee 19 for(int i = 0; i < 7; i++) {
number_key 0:875532e626ee 20 button1[i] = data[4] % 2;
number_key 0:875532e626ee 21 data[4] /= 2;
number_key 0:875532e626ee 22 }
number_key 0:875532e626ee 23 for(int i=0; i<6; i++) {
number_key 0:875532e626ee 24 button2[i] = data[5] % 2;
number_key 0:875532e626ee 25 data[5] /= 2;
number_key 0:875532e626ee 26 }
number_key 0:875532e626ee 27 for(int i = 0; i < 4; i++) {
number_key 0:875532e626ee 28 stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0);
number_key 0:875532e626ee 29 }
number_key 0:875532e626ee 30 setStick();
number_key 0:875532e626ee 31 } else if(fepTemp == FEP_NO_RESPONSE) {
number_key 0:875532e626ee 32 return 0;
number_key 0:875532e626ee 33 } else {
number_key 0:875532e626ee 34 return 0;
number_key 0:875532e626ee 35 }
number_key 0:875532e626ee 36 return 1;
number_key 0:875532e626ee 37 }
number_key 0:875532e626ee 38
number_key 0:875532e626ee 39 void Controller::setStick()
number_key 0:875532e626ee 40 {
number_key 0:875532e626ee 41 for(int i = 0; i < 4; i++) {
number_key 0:875532e626ee 42 if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0;
number_key 0:875532e626ee 43 }
number_key 0:875532e626ee 44
number_key 0:875532e626ee 45 radian[0] = atan2(stick[1], -stick[0]);
number_key 0:875532e626ee 46 radian[1] = atan2(stick[3], -stick[2]);
number_key 0:875532e626ee 47
number_key 0:875532e626ee 48 norm[0] = hypot(stick[0], stick[1]);
number_key 0:875532e626ee 49 norm[1] = hypot(stick[2], stick[3]);
number_key 0:875532e626ee 50
number_key 0:875532e626ee 51 if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
number_key 0:875532e626ee 52 if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
number_key 0:875532e626ee 53 if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
number_key 0:875532e626ee 54 if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;
number_key 0:875532e626ee 55 }
number_key 0:875532e626ee 56
number_key 0:875532e626ee 57 bool Controller::getButton1(int number) const
number_key 0:875532e626ee 58 {
number_key 0:875532e626ee 59 return button1[number];
number_key 0:875532e626ee 60 }
number_key 0:875532e626ee 61
number_key 0:875532e626ee 62 bool Controller::getButton2(int number) const
number_key 0:875532e626ee 63 {
number_key 0:875532e626ee 64 return button2[number];
number_key 0:875532e626ee 65 }
number_key 0:875532e626ee 66
number_key 0:875532e626ee 67 float Controller::getStick(int number) const
number_key 0:875532e626ee 68 {
number_key 0:875532e626ee 69 return stick[number];
number_key 0:875532e626ee 70 }
number_key 0:875532e626ee 71
number_key 0:875532e626ee 72 float Controller::getRadian(int number) const
number_key 0:875532e626ee 73 {
number_key 0:875532e626ee 74 return radian[number];
number_key 0:875532e626ee 75 }
number_key 0:875532e626ee 76
number_key 0:875532e626ee 77 float Controller::getNorm(int number) const
number_key 0:875532e626ee 78 {
number_key 0:875532e626ee 79 return norm[number];
number_key 0:875532e626ee 80 }