NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2

Octopus!!

Dependencies:   2017NHKpin_config FEP HMC6352 PID QEI R1307 ikarashiMDC omni_wheel

Fork of KANIv3 by NagaokaRoboticsClub_mbedTeam

bot/controller/controller.cpp@52:320f910ca6ca, 2017-11-23 (annotated)

Committer:
number_key
Date:
Thu Nov 23 11:49:39 2017 +0900
Revision:
52:320f910ca6ca
Parent:
49:69a7235d837a 
dirty

Who changed what in which revision?

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