NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated KANI2017v2

NHK2017Ateamかにろぼ

Dependencies:   2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni

classDiagram

　   ＼￣＼　　　　　　　　　　　　　    　 ／￣／ 
/l　    ＼　　＼　　 　　　　　　　　　 ／　　／　lヽ　 
|　ヽ 　ヽ　　 |　　　　 　　　　　　｜　　/　　/　| 
＼　` ‐ヽ　　ヽ　　●　　      ●    　　   /　　/　‐　 ／ 
　 ＼ __　l　　|　　｜|＿＿＿|｜ /　　l　__ ／ 
　　　 　＼　 ＼ ／　　　　　　＼／ 
　　　　　 ／＼|　　 人＿＿人　　|／＼　　　　　　 
　　　　／／＼|　　　　　      　　|／＼＼　　　　 
　　　　／／＼|　　　　　　      　|／＼＼　　　　 
　　　　／　  　 . ＼＿＿＿＿＿／　 　      ＼ 

　 　 　 　 　 　 　 　 　 　 　 　    　 　 ┏┓　 　 　 　 ┏━┓┏┓　 　 　 　 　 　 　 
     ┏┓　      　 ┏┓┏┓   ┏┓　 　 ┏┓┗┛   　 ┏┓　┗┓┃┗┛　 　 　 　 　 　 　 
┏┛┗━┓ 　┃┃┃┃    ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ 
┗┓┏━┛　 ┃┃┗┛    ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛　 　 
┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ 　    ┏┛┃┃┃  　 　 　 ┃┃　 　 　 　 　 　 　 
┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ 　    ┗━┛┃┃  　 　 　 ┃┃┏┓　 　 　 　 　 
┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃　 　  　 　 ┏┛┃  　 　 　 ┃┃┃┗━━┓　 　 
┗┛┗━━┛┗━━━┛┗┛┗━━┛　 　  　 　 ┗━┛　   　 　 ┗┛┗━━━┛　

bot/controller/controller.cpp@23:98746361f827, 2017-09-05 (annotated)

Committer:
uchitake
Date:
Tue Sep 05 12:02:47 2017 +0900
Revision:
23:98746361f827
Parent:
22:bb6afe7332c3 
remove WheelUnit

Who changed what in which revision?

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