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@5:bc5ba4f070ad, 2017-08-24 (annotated)

Committer:
UCHITAKE
Date:
Thu Aug 24 08:16:09 2017 +0000
Revision:
5:bc5ba4f070ad
Parent:
2:ea151e05033a
Child:
9:cba664555161
Just worked !!!!!!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:757e9800c103 1 #include "controller.h"
UCHITAKE 0:757e9800c103 2
UCHITAKE 0:757e9800c103 3 Controller::Controller() : FEP(controllerTX, controllerRX, ADDR)
UCHITAKE 0:757e9800c103 4 {
UCHITAKE 1:269914e0aa07 5 for(int i = 0; i < 7; i++) Button1[i] = 0;
UCHITAKE 1:269914e0aa07 6 for(int i = 0; i < 6; i++) Button2[i] = 0;
UCHITAKE 1:269914e0aa07 7 for(int i = 0; i < 6; i++) data[i] = 0;
UCHITAKE 1:269914e0aa07 8 for(int i = 0; i < 4; i++) stick[i] = 0;
UCHITAKE 1:269914e0aa07 9 for(int i = 0; i < 2; i++) degree[i] = 0;
UCHITAKE 1:269914e0aa07 10 for(int i = 0; i < 2; i++) norm[i] = 0;
UCHITAKE 1:269914e0aa07 11 for(int i = 0; i < 2; i++) vector[i] = 0;
UCHITAKE 1:269914e0aa07 12 fepTemp = 0;
UCHITAKE 1:269914e0aa07 13 }
UCHITAKE 1:269914e0aa07 14
UCHITAKE 1:269914e0aa07 15 void Controller::receiveState()
UCHITAKE 1:269914e0aa07 16 {
UCHITAKE 1:269914e0aa07 17 fepTemp = read(data, 6);
UCHITAKE 1:269914e0aa07 18 if(fepTemp == FEP_SUCCESS) {
UCHITAKE 1:269914e0aa07 19 for(int i=0; i<7; i++) {
UCHITAKE 1:269914e0aa07 20 Button1[i] = data[4] % 2;
UCHITAKE 1:269914e0aa07 21 data[4] /= 2;
UCHITAKE 1:269914e0aa07 22 }
UCHITAKE 1:269914e0aa07 23 for(int i=0; i<6; i++) {
UCHITAKE 1:269914e0aa07 24 Button2[i] = data[5] % 2;
UCHITAKE 1:269914e0aa07 25 data[5] /= 2;
UCHITAKE 1:269914e0aa07 26 }
UCHITAKE 1:269914e0aa07 27 for(int i = 0; i < 4; i++) {
UCHITAKE 1:269914e0aa07 28 stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0);
UCHITAKE 1:269914e0aa07 29 }
UCHITAKE 1:269914e0aa07 30 getStickData();
UCHITAKE 1:269914e0aa07 31 } else if(fepTemp == FEP_NO_RESPONSE) {
UCHITAKE 1:269914e0aa07 32 } else {
UCHITAKE 1:269914e0aa07 33 }
UCHITAKE 1:269914e0aa07 34 }
UCHITAKE 1:269914e0aa07 35
UCHITAKE 1:269914e0aa07 36 void Controller::getStickData()
UCHITAKE 1:269914e0aa07 37 {
UCHITAKE 5:bc5ba4f070ad 38 for(int i = 0; i < 4; i++) {
UCHITAKE 5:bc5ba4f070ad 39 if(stick[i] < 0.1 && stick[i] > -0.1) stick[i] = 0;
UCHITAKE 5:bc5ba4f070ad 40 }
UCHITAKE 1:269914e0aa07 41 degree[0] = (atan2(stick[1], -stick[0])) * (180 / M_PI);
UCHITAKE 1:269914e0aa07 42 degree[1] = (atan2(stick[3], -stick[2])) * (180 / M_PI);
UCHITAKE 1:269914e0aa07 43
UCHITAKE 1:269914e0aa07 44 norm[0] = hypot(stick[0], stick[1]);
UCHITAKE 1:269914e0aa07 45 norm[1] = hypot(stick[2], stick[3]);
UCHITAKE 1:269914e0aa07 46
UCHITAKE 1:269914e0aa07 47 if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
UCHITAKE 1:269914e0aa07 48 if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
UCHITAKE 1:269914e0aa07 49 if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
UCHITAKE 1:269914e0aa07 50 if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;
UCHITAKE 1:269914e0aa07 51
UCHITAKE 1:269914e0aa07 52 vector[0] = degree[0];
UCHITAKE 1:269914e0aa07 53 vector[1] = norm[0];
UCHITAKE 2:ea151e05033a 54 vector2[0] = degree[1];
UCHITAKE 2:ea151e05033a 55 vector2[1] = norm[1];
UCHITAKE 0:757e9800c103 56 }