main.cpp

00001 #include "mbed.h"
00002 char *tmp[2];
00003 char *SenseTmp[2];
00004 #include "Nunchuck.h"
00005 #include "defines.h"
00006 #include "param_motion.h"
00007 Serial dev(D1,D0);
00008 Serial sbdbt(D13,D12);
00009 #include "methods.h"
00010 
00011 DigitalOut debugLed1(D6);
00012 DigitalOut debugLed2(D7);
00013 bool send = false;
00014 
00015 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
00016 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
00017 bool ReverseL[4] = {true,false,true,true};
00018 bool ReverseR[4] = {false,false,false,true};
00019 Timer timer;
00020 void RX(){
00021     if(dev.getc() == '0'){
00022         timer.reset();
00023         debugLed1 = true;
00024         for(int i = 1 ; i < dataNum;i++){
00025             RXData[i] = dev.getc();
00026         }
00027         //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
00028         for(int i = 0 ;i < 4 ; i++){
00029             floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
00030             in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
00031             in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
00032             uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
00033             uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
00034             intt = ReverseL[i] == true ? 0xffff - intt : intt;
00035             floatInByte intt_;
00036             intt_.si = intt;
00037             //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d  ",intt);
00038             //uint16_t intt = map(in_,13107,52428,0,65535);
00039             ArmInputSense[L][i].read_u16(float(intt_.si)/0xffff);
00040             RXData[4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
00041             RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
00042         }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
00043             
00044         timer.reset();
00045         send = true;
00046     }
00047 }
00048 void print(int N,char RXdata[12]){
00049     floatInByte data;
00050     for(int i = 0 ; i < 2 ; i ++){
00051         data.c[i] = RXdata[N+i];
00052     }
00053     sbdbt.printf("%d ",data.si);
00054 }
00055 int id = 0;
00056 int main() {
00057 
00058     dev.baud(115200);
00059     
00060     sbdbt.baud(115200);
00061     for(int i = 0 ; i < 2; i++)
00062     {
00063         tmp[i] = new char[dataNum]; 
00064     }
00065     debugLed1 = true;
00066     for(int i = 0 ; i < 50 ; i ++ ){
00067         debugLed2 = !debugLed2;
00068         wait(0.1);
00069     }
00070     dev.attach(RX, Serial::RxIrq);
00071     dev.putc('L');
00072     timer.start();
00073     motionTimer.start();
00074     while(1) {
00075         //送信データ格納
00076         
00077         ctrl.getdata();
00078         tmp[R][0] = 'H';
00079         tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
00080         tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
00081         tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
00082         for(int i = 0 ;i < 4 ; i++){
00083             floatInByte intt_;
00084             //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
00085             uint16_t in = ArmSense[i].read_u16();
00086             //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d  ",intt);
00087             uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
00088             intt = ReverseR[i] == true ? 0xffff - intt : intt;
00089             intt_.si = intt;
00090             ArmInputSense[R][i].read_u16(float(intt_.si)/0xffff);
00091             //uint16_t intt = map(in_,13107,52428,0,65535);
00092             tmp[R][4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
00093             tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
00094         }
00095         tmp[L] = (char*)RXData;
00096         signed char x1 = map8(tmp[R][1],-90,90,-100,100)+1;
00097         signed char y1 = map8(tmp[R][2],-90,90,-100,100)-7;
00098         signed char x2 = map8(tmp[L][1],-90,90,-100,100)+3;
00099         signed char y2 = map8(tmp[L][2],-90,90,-100,100)-10;
00100         //sbdbt.printf("%d %d %d %d ",x1,y1,x2,y2);
00101         sbdbt.printf("sita %d ",impulse(float(y2)/100.0F));
00102         char** SerialData = ;
00103         bool txFlag = true;
00104         switch(int i = MotionSense()){
00105             case NOTINPUT:
00106                 //tmp[L] = (char*)RXData;
00107                 //SerialData = tmp;
00108             break;
00109             case LOADMOTION:    
00110                 //sbdbt.printf("LOADMOTION");
00111                 //tmp[L] = (char*)RXData;
00112                 //SerialData = tmp;
00113             break;
00114             case TIMEOUT:
00115                 //sbdbt.printf("TIMEOUT");
00116                 //tmp[L] = (char*)RXData;
00117                 //SerialData = tmp;
00118             break;
00119             default:
00120                 id = i;
00121                 sbdbt.printf("GET %d ",i);
00122                 txFlag = false;
00123             break;
00124         }
00125         //if(playMotionFlag == true){
00126             playMotion(0,(uint8_t**)SerialData);
00127         //}
00128         //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
00129         sbdbt.printf("\n");
00130         //送信データを送る
00131         //SerialData = tmp;
00132         if(send == true || txFlag == true){
00133             for(int j = 0; j < 2 ; j++){
00134                 for(int i = 0 ; i < dataNum ; i++){
00135                     //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
00136                     //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
00137                 }
00138             }
00139             //sbdbt.printf("R:");
00140             //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
00141             //sbdbt.printf("L:");
00142             //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
00143              for(int j = 0; j < 2 ; j++){
00144                 for(int i = 0 ; i < dataNum ; i++){
00145                     sbdbt.printf("%3d ",SerialData[j][i]);
00146                     //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
00147                 }
00148             }
00149             if(!DEBUG)sbdbt.printf("\n");
00150         }
00151         dev.putc('L');
00152         send = false;
00153         while(timer.read_ms() >= 2000){
00154             debugLed2 = true;
00155             waitTime(0.1);
00156             debugLed2 = false;
00157             waitTime(0.1);
00158         }
00159         debugLed1 = false;
00160         waitTime(0.01);
00161         #if DEBUG
00162             int RL = R;
00163             //if(DEBUG_R){sbdbt.printf("R:");RL = R;}
00164             //if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
00165             //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
00166             //sbdbt.printf("\n");
00167         #endif
00168     }
00169 }