controller.cpp

00001 #include "controller.h"
00002 
00003 Controller::Controller(PinName FEPtx, PinName FEPrx, int addr) :
00004     FEP(FEPtx, FEPrx, addr),
00005     data(),
00006     fepTemp(0),
00007     button1(),
00008     button2(),
00009     stick(),
00010     radian(),
00011     norm()
00012 {
00013 }
00014 
00015 bool Controller::receiveState()
00016 {
00017     fepTemp = FEP::read(data, DATA_SIZE);
00018     if(fepTemp == FEP_SUCCESS) {
00019         for(int i = 0; i < 7; i++) {
00020             button1[i] = data[4] % 2;
00021             data[4] /= 2;
00022         }
00023         for(int i=0; i<6; i++) {
00024             button2[i] = data[5] % 2;
00025             data[5] /= 2;
00026         }
00027         for(int i = 0; i < 4; i++) {
00028             stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0);
00029         }
00030         setStick();
00031     } else if(fepTemp == FEP_NO_RESPONSE) {
00032         return 0;
00033     } else {
00034         return 0;
00035     }
00036     return 1;
00037 }
00038 
00039 void Controller::setStick()
00040 {
00041     for(int i = 0; i < 4; i++) {
00042         if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0;
00043     }
00044 
00045     radian[0] = atan2(stick[1], -stick[0]);
00046     radian[1] = atan2(stick[3], -stick[2]);
00047 
00048     norm[0] = hypot(stick[0], stick[1]);
00049     norm[1] = hypot(stick[2], stick[3]);
00050 
00051     if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
00052     if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
00053     if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
00054     if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;
00055 }
00056 
00057 bool Controller::getButton1(int number) const
00058 {
00059     return button1[number];
00060 }
00061 
00062 bool Controller::getButton2(int number) const
00063 {
00064     return button2[number];
00065 }
00066 
00067 float Controller::getStick(int number) const
00068 {
00069     return stick[number];
00070 }
00071 
00072 float Controller::getRadian(int number) const
00073 {
00074     return radian[number];
00075 }
00076 
00077 float Controller::getNorm(int number) const
00078 {
00079     return norm[number];
00080 }