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@1:845af5425eec, 2017-09-05 (annotated)

Committer:
uchitake
Date:
Tue Sep 05 16:11:20 2017 +0900
Revision:
1:845af5425eec
Child:
15:9aa11febe517
mbed-os5

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() :
uchitake 1:845af5425eec 4 FEP(XBee1TX, XBee1RX, ADDR),
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
uchitake 1:845af5425eec 27 bool 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);
uchitake 1:845af5425eec 41 }
uchitake 1:845af5425eec 42 setStick();
uchitake 1:845af5425eec 43 } else if(fepTemp == FEP_NO_RESPONSE) {
uchitake 1:845af5425eec 44 return 0;
uchitake 1:845af5425eec 45 } else {
uchitake 1:845af5425eec 46 return 0;
uchitake 1:845af5425eec 47 }
uchitake 1:845af5425eec 48 return 1;
uchitake 1:845af5425eec 49 }
uchitake 1:845af5425eec 50
uchitake 1:845af5425eec 51 void Controller::setStick()
uchitake 1:845af5425eec 52 {
uchitake 1:845af5425eec 53 for(int i = 0; i < 4; i++) {
uchitake 1:845af5425eec 54 if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0;
uchitake 1:845af5425eec 55 }
uchitake 1:845af5425eec 56
uchitake 1:845af5425eec 57 radian[0] = atan2(stick[1], -stick[0]);
uchitake 1:845af5425eec 58 radian[1] = atan2(stick[3], -stick[2]);
uchitake 1:845af5425eec 59
uchitake 1:845af5425eec 60 norm[0] = hypot(stick[0], stick[1]);
uchitake 1:845af5425eec 61 norm[1] = hypot(stick[2], stick[3]);
uchitake 1:845af5425eec 62
uchitake 1:845af5425eec 63 if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
uchitake 1:845af5425eec 64 if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
uchitake 1:845af5425eec 65 if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
uchitake 1:845af5425eec 66 if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;
uchitake 1:845af5425eec 67 }
uchitake 1:845af5425eec 68
uchitake 1:845af5425eec 69 bool Controller::getButton1(int number) const
uchitake 1:845af5425eec 70 {
uchitake 1:845af5425eec 71 return button1[number];
uchitake 1:845af5425eec 72 }
uchitake 1:845af5425eec 73
uchitake 1:845af5425eec 74 bool Controller::getButton2(int number) const
uchitake 1:845af5425eec 75 {
uchitake 1:845af5425eec 76 return button2[number];
uchitake 1:845af5425eec 77 }
uchitake 1:845af5425eec 78
uchitake 1:845af5425eec 79 float Controller::getStick(int number) const
uchitake 1:845af5425eec 80 {
uchitake 1:845af5425eec 81 return stick[number];
uchitake 1:845af5425eec 82 }
uchitake 1:845af5425eec 83
uchitake 1:845af5425eec 84 float Controller::getRadian(int number) const
uchitake 1:845af5425eec 85 {
uchitake 1:845af5425eec 86 return radian[number];
uchitake 1:845af5425eec 87 }
uchitake 1:845af5425eec 88
uchitake 1:845af5425eec 89 float Controller::getNorm(int number) const
uchitake 1:845af5425eec 90 {
uchitake 1:845af5425eec 91 return norm[number];
uchitake 1:845af5425eec 92 }