main.cpp

00001 #include "mbed.h"
00002 #include "Nunchuck.h"
00003 #define R 0
00004 #define L 1
00005 
00006 #define DEBUG 0
00007 #define DEBUG_R 1
00008 #define CALIB 0
00009 
00010 #define SetupCMD 'A'
00011 #define Respons 'S'
00012 DigitalOut debugLed1(D6);
00013 DigitalOut debugLed2(D7);
00014 bool send = false;
00015 union floatInByte
00016 {
00017     uint16_t si;
00018     unsigned char c[2];
00019 };
00020 class AnalogInLPF : public AnalogIn
00021 {
00022     private:
00023     float alpha;
00024     float prevAnalog;
00025     float nowAnalog;
00026     public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin)
00027     {
00028         alpha = alpha_;
00029         prevAnalog = 0.0;
00030     } 
00031     float read(){
00032         nowAnalog = AnalogIn::read();
00033         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00034         prevAnalog = nowAnalog;
00035         return nowAnalog;
00036     }
00037     short read_u16(){
00038         nowAnalog = AnalogIn::read();
00039         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00040         nowAnalog = float(short(nowAnalog*0xFFFF)&0xFF00)/0xFFFF;
00041         prevAnalog = nowAnalog;
00042         return short(nowAnalog*0xFFFF);
00043     }
00044 };
00045 class InLPF
00046 {
00047     private:
00048     float alpha;
00049     float prevAnalog;
00050     float nowAnalog;
00051     public : InLPF(float alpha_ = 0.2)
00052     {
00053         alpha = alpha_;
00054         prevAnalog = 0.0;
00055     } 
00056     float read(float in){
00057         nowAnalog = in;
00058         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00059         prevAnalog = nowAnalog;
00060         return nowAnalog;
00061     }
00062     short read_u16(float in){
00063         nowAnalog = in;
00064         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00065         prevAnalog = nowAnalog;
00066         return short(nowAnalog*0xFFFF)&0xFF00;
00067     }
00068 };
00069 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax);
00070 #define LP 0.30
00071 AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP-0.018),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)};
00072 InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)};
00073 #if !CALIB
00074 //1号機
00075 /*
00076 uint16_t MinimumRangeR[4] = {12800,41414,26368,11776};
00077 uint16_t MaxmumRangeR[4] = {36608,52242,55552,38144};
00078 uint16_t MinimumRangeL[4] = {23040,24832,24576,32256};
00079 uint16_t MaxmumRangeL[4] = {44312,47296,59392,58624};
00080 
00081 bool ReverseL[4] = {false,true,true,false};
00082 bool ReverseR[4] = {true,false,true,false};
00083 */
00084 /*
00085 uint16_t MinimumRangeR[4] = {14592,41414,16128,12288};
00086 uint16_t MaxmumRangeR[4] = {41216,52242,61952,38144};
00087 uint16_t MinimumRangeL[4] = {23040,18176,11008,34303};
00088 uint16_t MaxmumRangeL[4] = {44312,47296,60106,58368};
00089 
00090 bool ReverseL[4] = {false,true,true,false};
00091 bool ReverseR[4] = {true,false,true,false};
00092 */
00093 /*
00094 uint16_t MinimumRangeR[4] = {1280,42240,36096,17152};
00095 uint16_t MaxmumRangeR[4] = {45312,52992,58624,41216};
00096 uint16_t MinimumRangeL[4] = {23040,18432,24576,58112};
00097 uint16_t MaxmumRangeL[4] = {45312,58112,59392,30976};
00098 */
00099 //2号機
00100 //17468 23308
00101 uint16_t MinimumRangeR[4] = {19900,41216,45056,13002};
00102 uint16_t MaxmumRangeR[4] = {42544,53248,60676,38312};
00103 uint16_t MinimumRangeL[4] = {21500,19432,10190,55588};
00104 uint16_t MaxmumRangeL[4] = {43520,43300,43264,30208};
00105 
00106 bool ReverseL[4] = {false,true,true,true};
00107 bool ReverseR[4] = {true,false,true,false};
00108 
00109 #endif
00110 #if CALIB
00111 uint16_t MinimumRangeR[4] = {0,0,0,0};
00112 uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff};
00113 uint16_t MinimumRangeL[4] = {0,0,0,0};
00114 uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff};
00115 #endif
00116 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
00117 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
00118 //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)};
00119 Nunchuck ctrl(D4,D5);
00120 Serial dev(D1,D0);
00121 Serial sbdbt(D13,D12);
00122 #define dataNum 12
00123 void waitTime(float ti){
00124     Timer t;
00125     t.start();
00126     while(ti > t.read());
00127     t.stop();
00128     return;
00129 }
00130 Timer timer;
00131 char *tmp[2];
00132 char RXData[dataNum] = {'0'}; 
00133 void RX(){
00134     if(dev.getc() == '0'){
00135         timer.reset();
00136         debugLed1 = true;
00137         for(int i = 1 ; i < dataNum;i++){
00138             RXData[i] = dev.getc();
00139         }
00140         //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
00141         for(int i = 0 ;i < 4 ; i++){
00142             floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
00143             in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
00144             in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
00145             uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
00146             //uint16_t intt = map(in_,ReverseL[i] == true ? 0xffff - MaxmumRangeL[i] : MinimumRangeL[i],ReverseL[i] == true ? 0xffff - MinimumRangeL[i] : MaxmumRangeL[i],0,65535);
00147             uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
00148             #if !CALIB
00149             intt = ReverseL[i] == true ? 0xffff - intt : intt;
00150             #endif
00151             floatInByte intt_;
00152             intt_.si = intt;
00153             //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d  ",intt);
00154             //uint16_t intt = map(in_,13107,52428,0,65535);
00155             RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
00156             RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
00157         }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
00158             
00159         timer.reset();
00160         send = true;
00161     }
00162 }
00163 void print(int N,char RXdata[12]){
00164     floatInByte data;
00165     for(int i = 0 ; i < 2 ; i ++){
00166         data.c[i] = RXdata[N+i];
00167     }
00168     sbdbt.printf("%d ",data.si);
00169 }
00170 double offset[4] = {0,0,0,-132};
00171 double range[4] = {120,120,90,240};
00172 double range2[4] = {120,120,90,9};
00173 int main() {
00174 
00175     dev.baud(115200);
00176     
00177     sbdbt.baud(115200);
00178     for(int i = 0 ; i < 2; i++)
00179     {
00180         tmp[i] = new char[dataNum]; 
00181     }
00182     debugLed1 = true;
00183     for(int i = 0 ; i < 10 ; i ++ ){
00184         debugLed2 = !debugLed2;
00185         wait(0.01);
00186     }
00187     wait(0.5);
00188     dev.attach(RX, Serial::RxIrq);
00189     dev.putc('L');
00190     timer.start();        
00191     ctrl.offset_();
00192     int count = 0;
00193     while(1) {
00194         //送信データ格納
00195         tmp[R][0] = 'H';    
00196         tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
00197         tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
00198         tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
00199 
00200         for(int i = 0 ;i < 4 ; i++){
00201             floatInByte intt_;
00202             uint16_t in = ArmSense[i].read_u16();
00203             uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
00204             #if !CALIB
00205             intt = ReverseR[i] == true ? 0xffff - intt : intt;
00206             #endif 
00207             intt_.si = intt;
00208             tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
00209             tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
00210         }
00211         //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
00212         tmp[L] = RXData;
00213         char** SerialData = tmp;
00214         
00215         //送信データを送る
00216         //SerialData = tmp;
00217         if(count > 20 && send == true){
00218             ctrl.getdata();
00219             #if CALIB
00220             sbdbt.printf("R:");
00221             for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
00222             sbdbt.printf("L:");
00223             for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
00224             #endif
00225             for(int j = 0; j < 2 ; j++){
00226                 for(int i = 0 ; i < dataNum ; i++){
00227                     //sbdbt.printf("%3d ",int8_t(SerialData[j][i]));
00228                     #if !CALIB
00229                         if(!DEBUG)sbdbt.putc(SerialData[j][i]);
00230                     #endif
00231                 }
00232             }
00233             count = 0;
00234         if(!DEBUG)sbdbt.printf("\n");
00235         }
00236         dev.putc('L');
00237         send = false;
00238         while(timer.read_ms() >= 2000){
00239             debugLed2 = true;
00240             waitTime(0.1);
00241             debugLed2 = false;
00242             waitTime(0.1);
00243         }
00244         debugLed1 = false;
00245         waitTime(1.0/1000);
00246         count ++;
00247     }
00248 }
00249 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
00250   // check it's within the range
00251   if (inMin<inMax) { 
00252     if (in <= inMin) 
00253       return outMin;
00254     if (in >= inMax)
00255       return outMax;
00256   } else {  // cope with input range being backwards.
00257     if (in >= inMin) 
00258       return outMin;
00259     if (in <= inMax)
00260       return outMax;
00261   }
00262   // calculate how far into the range we are
00263   float scale = float(in-inMin)/float(inMax-inMin);
00264   // calculate the output.
00265   return uint16_t(outMin + scale*float(outMax-outMin));
00266 }