use for experiment before the demonstration at open-campus
Dependencies: FEP ikarashiMDC omni PID R1370
Fork of omni_sample by
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?
User | Revision | Line number | New 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 | } |