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@49:69a7235d837a, 2017-11-11 (annotated)

Committer:
number_key
Date:
Sat Nov 11 17:40:10 2017 +0900
Revision:
49:69a7235d837a
Parent:
43:c659cd5baa14
Child:
52:320f910ca6ca
save

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