main2.cpp

00001 #if 1
00002 #include "mbed.h"
00003 #include "Nunchuck.h"
00004 #define R 0
00005 #define L 1
00006 
00007 #define DEBUG 0
00008 #define DEBUG_R 1
00009 #define CALIB 0
00010 
00011 #define SetupCMD 'A'
00012 #define Respons 'S'
00013 DigitalOut debugLed1(D6);
00014 DigitalOut debugLed2(D7);
00015 bool send = false;
00016 union floatInByte
00017 {
00018     uint16_t si;
00019     unsigned char c[2];
00020 };
00021 class AnalogInLPF : public AnalogIn
00022 {
00023     private:
00024     float alpha;
00025     float prevAnalog;
00026     float nowAnalog;
00027     public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin)
00028     {
00029         alpha = alpha_;
00030         prevAnalog = 0.0;
00031     } 
00032     float read(){
00033         nowAnalog = AnalogIn::read();
00034         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00035         prevAnalog = nowAnalog;
00036         return nowAnalog;
00037     }
00038     short read_u16(){
00039         nowAnalog = AnalogIn::read();
00040         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
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);
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.7
00071 AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP),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 //uint16_t MinimumRangeR[4] = {4000,40000,53500,16000};
00076 //uint16_t MaxmumRangeR[4] = {55000,51000,58900,45000};
00077 //uint16_t MinimumRangeL[4] = {31500,8000,30000,19800};
00078 //uint16_t MaxmumRangeL[4] = {48000,46400,46200,45000};
00079 //2号機
00080 /*
00081 uint16_t MinimumRangeR[4] = {0xffff-43000,48000,52100,0xffff-52000};
00082 uint16_t MaxmumRangeR[4] = {0xffff-29800,53300,56000,0xffff-28400};
00083 uint16_t MinimumRangeL[4] = {62900,11300,37700,14200};
00084 uint16_t MaxmumRangeL[4] = {45800,56000,27900,44000};
00085 */
00086 
00087 uint16_t MinimumRangeR[4] = {19900,17664,45056,13002};
00088 uint16_t MaxmumRangeR[4] = {42544,39424,60676,38312};
00089 uint16_t MinimumRangeL[4] = {21500,19432,10190,55588};
00090 uint16_t MaxmumRangeL[4] = {43520,43300,43264,30208};
00091 
00092 bool ReverseL[4] = {false,true,true,true};
00093 bool ReverseR[4] = {true,false,true,false};
00094 #endif
00095 #if CALIB
00096 uint16_t MinimumRangeR[4] = {0,0,0,0};
00097 uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff};
00098 uint16_t MinimumRangeL[4] = {0,0,0,0};
00099 uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff};
00100 #endif
00101 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
00102 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
00103 //bool ReverseL[4] = {true,false,true,true};
00104 //bool ReverseR[4] = {true,false,false,false};
00105 //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)};
00106 Nunchuck ctrl(D4,D5);
00107 Serial dev(D1,D0);
00108 Serial sbdbt(D13,D12);
00109 #define dataNum 12
00110 void waitTime(float ti){
00111     Timer t;
00112     t.start();
00113     while(ti > t.read());
00114     t.stop();
00115     return;
00116 }
00117 Timer timer;
00118 char *tmp[2];
00119 char RXData[dataNum] = {'0'}; 
00120 void RX(){
00121     if(dev.getc() == '0'){
00122         timer.reset();
00123         debugLed1 = true;
00124         for(int i = 1 ; i < dataNum;i++){
00125             RXData[i] = dev.getc();
00126         }
00127         //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
00128         for(int i = 0 ;i < 4 ; i++){
00129             floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
00130             in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
00131             in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
00132             uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
00133             uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
00134             intt = ReverseL[i] == true ? 0xffff - intt : intt;
00135             floatInByte intt_;
00136             intt_.si = intt;
00137             //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d  ",intt);
00138             //uint16_t intt = map(in_,13107,52428,0,65535);
00139             RXData[4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
00140             RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
00141         }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
00142             
00143         timer.reset();
00144         send = true;
00145     }
00146 }
00147 void print(int N,char RXdata[12]){
00148     floatInByte data;
00149     for(int i = 0 ; i < 2 ; i ++){
00150         data.c[i] = RXdata[N+i];
00151     }
00152     sbdbt.printf("%d ",data.si);
00153 }
00154 double offset[4] = {0,0,0,-132};
00155 double range[4] = {120,120,90,240};
00156 double range2[4] = {120,120,90,9};
00157 int main() {
00158 
00159     dev.baud(115200);
00160     
00161     sbdbt.baud(115200);
00162     for(int i = 0 ; i < 2; i++)
00163     {
00164         tmp[i] = new char[dataNum]; 
00165     }
00166     debugLed1 = true;
00167     for(int i = 0 ; i < 50 ; i ++ ){
00168         debugLed2 = !debugLed2;
00169         wait(0.1);
00170     }
00171     dev.attach(RX, Serial::RxIrq);
00172     dev.putc('L');
00173     timer.start();
00174     while(1) {
00175         //送信データ格納
00176         
00177         
00178         tmp[R][0] = 'H';
00179         tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
00180         tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
00181         tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
00182         for(int i = 0 ;i < 4 ; i++){
00183             floatInByte intt_;
00184             //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
00185             uint16_t in = ArmSense[i].read_u16();
00186             //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d  ",intt);
00187             uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
00188             intt = ReverseR[i] == true ? 0xffff - intt : intt;
00189             intt_.si = intt;
00190             //uint16_t intt = map(in_,13107,52428,0,65535);
00191             tmp[R][4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
00192             tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
00193         }
00194         //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
00195         tmp[L] = RXData;
00196         char** SerialData = tmp;
00197         
00198         //送信データを送る
00199         //SerialData = tmp;
00200         if(send == true){
00201             for(int j = 0; j < 2 ; j++){
00202                 for(int i = 0 ; i < dataNum ; i++){
00203                     //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
00204                     //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
00205                 }
00206             }
00207             /*
00208             sbdbt.printf("R:");
00209             for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
00210             sbdbt.printf("L:");
00211             for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
00212             */
00213             for(int j = 0; j < 2 ; j++){
00214                 for(int i = 0 ; i < dataNum ; i++){
00215                     //sbdbt.printf("%3d ",SerialData[j][i]);
00216                     if(!DEBUG)sbdbt.putc(SerialData[j][i]);
00217                 }
00218             }
00219         if(!DEBUG)sbdbt.printf("\n");
00220         }
00221         dev.putc('L');
00222         send = false;
00223         while(timer.read_ms() >= 2000){
00224             debugLed2 = true;
00225             waitTime(0.1);
00226             debugLed2 = false;
00227             waitTime(0.1);
00228         }
00229         debugLed1 = false;
00230         waitTime(0.01);
00231         #if DEBUG
00232             int RL = R;
00233             if(DEBUG_R){sbdbt.printf("R:");RL = R;}
00234             if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
00235             for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
00236             sbdbt.printf("\n");
00237         #endif
00238     }
00239 }
00240 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
00241   // check it's within the range
00242   if (inMin<inMax) { 
00243     if (in <= inMin) 
00244       return outMin;
00245     if (in >= inMax)
00246       return outMax;
00247   } else {  // cope with input range being backwards.
00248     if (in >= inMin) 
00249       return outMin;
00250     if (in <= inMax)
00251       return outMax;
00252   }
00253   // calculate how far into the range we are
00254   float scale = float(in-inMin)/float(inMax-inMin);
00255   // calculate the output.
00256   return uint16_t(outMin + scale*float(outMax-outMin));
00257 }
00258 #endif