NagaokaRoboticsClub_mbedTeam / Mbed OS quadOmni_yanagi

use for experiment before the demonstration at open-campus

Dependencies:   FEP ikarashiMDC omni PID R1370

Fork of omni_sample by NagaokaRoboticsClub_mbedTeam

controller/controller.cpp@16:7eaf3343f45e, 2017-09-15 (annotated)

Committer:
tkNara
Date:
Fri Sep 15 10:14:41 2017 +0900
Revision:
16:7eaf3343f45e
Parent:
9:8f9607783d2d 
update FEP baud 115200

Who changed what in which revision?

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