Kiko Ishimoto
/
robocon2017mbed_contoroler_L2
コントローラー
main.cpp
- Committer:
- kikoaac
- Date:
- 2017-10-27
- Revision:
- 8:022e458a1a45
- Parent:
- 7:e972d813367d
File content as of revision 8:022e458a1a45:
#include "mbed.h" #include "Nunchuck.h" #define R 0 #define L 1 #define DEBUG 0 #define DEBUG_R 1 #define CALIB 0 #define SetupCMD 'A' #define Respons 'S' DigitalOut debugLed1(D6); DigitalOut debugLed2(D7); bool send = false; union floatInByte { uint16_t si; unsigned char c[2]; }; class AnalogInLPF : public AnalogIn { private: float alpha; float prevAnalog; float nowAnalog; public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin) { alpha = alpha_; prevAnalog = 0.0; } float read(){ nowAnalog = AnalogIn::read(); nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; prevAnalog = nowAnalog; return nowAnalog; } short read_u16(){ nowAnalog = AnalogIn::read(); nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; nowAnalog = float(short(nowAnalog*0xFFFF)&0xFF00)/0xFFFF; prevAnalog = nowAnalog; return short(nowAnalog*0xFFFF); } }; class InLPF { private: float alpha; float prevAnalog; float nowAnalog; public : InLPF(float alpha_ = 0.2) { alpha = alpha_; prevAnalog = 0.0; } float read(float in){ nowAnalog = in; nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; prevAnalog = nowAnalog; return nowAnalog; } short read_u16(float in){ nowAnalog = in; nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; prevAnalog = nowAnalog; return short(nowAnalog*0xFFFF)&0xFF00; } }; uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax); #define LP 0.30 AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP-0.018),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)}; InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)}; #if !CALIB //1号機 /* uint16_t MinimumRangeR[4] = {12800,41414,26368,11776}; uint16_t MaxmumRangeR[4] = {36608,52242,55552,38144}; uint16_t MinimumRangeL[4] = {23040,24832,24576,32256}; uint16_t MaxmumRangeL[4] = {44312,47296,59392,58624}; bool ReverseL[4] = {false,true,true,false}; bool ReverseR[4] = {true,false,true,false}; */ /* uint16_t MinimumRangeR[4] = {14592,41414,16128,12288}; uint16_t MaxmumRangeR[4] = {41216,52242,61952,38144}; uint16_t MinimumRangeL[4] = {23040,18176,11008,34303}; uint16_t MaxmumRangeL[4] = {44312,47296,60106,58368}; bool ReverseL[4] = {false,true,true,false}; bool ReverseR[4] = {true,false,true,false}; */ /* uint16_t MinimumRangeR[4] = {1280,42240,36096,17152}; uint16_t MaxmumRangeR[4] = {45312,52992,58624,41216}; uint16_t MinimumRangeL[4] = {23040,18432,24576,58112}; uint16_t MaxmumRangeL[4] = {45312,58112,59392,30976}; */ //2号機 //17468 23308 uint16_t MinimumRangeR[4] = {19900,41216,45056,13002}; uint16_t MaxmumRangeR[4] = {42544,53248,60676,38312}; uint16_t MinimumRangeL[4] = {21500,19432,10190,55588}; uint16_t MaxmumRangeL[4] = {43520,43300,43264,30208}; bool ReverseL[4] = {false,true,true,true}; bool ReverseR[4] = {true,false,true,false}; #endif #if CALIB uint16_t MinimumRangeR[4] = {0,0,0,0}; uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff}; uint16_t MinimumRangeL[4] = {0,0,0,0}; uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff}; #endif //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000}; //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 }; //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)}; Nunchuck ctrl(D4,D5); Serial dev(D1,D0); Serial sbdbt(D13,D12); #define dataNum 12 void waitTime(float ti){ Timer t; t.start(); while(ti > t.read()); t.stop(); return; } Timer timer; char *tmp[2]; char RXData[dataNum] = {'0'}; void RX(){ if(dev.getc() == '0'){ timer.reset(); debugLed1 = true; for(int i = 1 ; i < dataNum;i++){ RXData[i] = dev.getc(); } //if(DEBUG && !DEBUG_R)sbdbt.printf("L:"); for(int i = 0 ;i < 4 ; i++){ floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2]; in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2]; in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2]; uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff); //uint16_t intt = map(in_,ReverseL[i] == true ? 0xffff - MaxmumRangeL[i] : MinimumRangeL[i],ReverseL[i] == true ? 0xffff - MinimumRangeL[i] : MaxmumRangeL[i],0,65535); uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535); #if !CALIB intt = ReverseL[i] == true ? 0xffff - intt : intt; #endif floatInByte intt_; intt_.si = intt; //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt); //uint16_t intt = map(in_,13107,52428,0,65535); RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕 RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕 }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n"); timer.reset(); send = true; } } void print(int N,char RXdata[12]){ floatInByte data; for(int i = 0 ; i < 2 ; i ++){ data.c[i] = RXdata[N+i]; } sbdbt.printf("%d ",data.si); } double offset[4] = {0,0,0,-132}; double range[4] = {120,120,90,240}; double range2[4] = {120,120,90,9}; int main() { dev.baud(115200); sbdbt.baud(115200); for(int i = 0 ; i < 2; i++) { tmp[i] = new char[dataNum]; } debugLed1 = true; for(int i = 0 ; i < 10 ; i ++ ){ debugLed2 = !debugLed2; wait(0.01); } wait(0.5); dev.attach(RX, Serial::RxIrq); dev.putc('L'); timer.start(); ctrl.offset_(); int count = 0; while(1) { //送信データ格納 tmp[R][0] = 'H'; tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン for(int i = 0 ;i < 4 ; i++){ floatInByte intt_; uint16_t in = ArmSense[i].read_u16(); uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535); #if !CALIB intt = ReverseR[i] == true ? 0xffff - intt : intt; #endif intt_.si = intt; tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕 tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕 } //if(DEBUG && DEBUG_R)sbdbt.printf("\n"); tmp[L] = RXData; char** SerialData = tmp; //送信データを送る //SerialData = tmp; if(count > 20 && send == true){ ctrl.getdata(); #if CALIB sbdbt.printf("R:"); for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]); sbdbt.printf("L:"); for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]); #endif for(int j = 0; j < 2 ; j++){ for(int i = 0 ; i < dataNum ; i++){ //sbdbt.printf("%3d ",int8_t(SerialData[j][i])); #if !CALIB if(!DEBUG)sbdbt.putc(SerialData[j][i]); #endif } } count = 0; if(!DEBUG)sbdbt.printf("\n"); } dev.putc('L'); send = false; while(timer.read_ms() >= 2000){ debugLed2 = true; waitTime(0.1); debugLed2 = false; waitTime(0.1); } debugLed1 = false; waitTime(1.0/1000); count ++; } } uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) { // check it's within the range if (inMin<inMax) { if (in <= inMin) return outMin; if (in >= inMax) return outMax; } else { // cope with input range being backwards. if (in >= inMin) return outMin; if (in <= inMax) return outMax; } // calculate how far into the range we are float scale = float(in-inMin)/float(inMax-inMin); // calculate the output. return uint16_t(outMin + scale*float(outMax-outMin)); }