Diff: main.cpp

Revision:

0:4a1a924f704e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 25 10:58:54 2017 +0000
@@ -0,0 +1,169 @@
+#include "mbed.h"
+char *tmp[2];
+char *SenseTmp[2];
+#include "Nunchuck.h"
+#include "defines.h"
+#include "param_motion.h"
+Serial dev(D1,D0);
+Serial sbdbt(D13,D12);
+#include "methods.h"
+
+DigitalOut debugLed1(D6);
+DigitalOut debugLed2(D7);
+bool send = false;
+
+//uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
+//uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
+bool ReverseL[4] = {true,false,true,true};
+bool ReverseR[4] = {false,false,false,true};
+Timer timer;
+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_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
+            intt = ReverseL[i] == true ? 0xffff - intt : intt;
+            floatInByte intt_;
+            intt_.si = intt;
+            //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d  ",intt);
+            //uint16_t intt = map(in_,13107,52428,0,65535);
+            ArmInputSense[L][i].read_u16(float(intt_.si)/0xffff);
+            RXData[4 + i*2] = 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);
+}
+int id = 0;
+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 < 50 ; i ++ ){
+        debugLed2 = !debugLed2;
+        wait(0.1);
+    }
+    dev.attach(RX, Serial::RxIrq);
+    dev.putc('L');
+    timer.start();
+    motionTimer.start();
+    while(1) {
+        //送信データ格納
+        
+        ctrl.getdata();
+        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_;
+            //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
+            uint16_t in = ArmSense[i].read_u16();
+            //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d  ",intt);
+            uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
+            intt = ReverseR[i] == true ? 0xffff - intt : intt;
+            intt_.si = intt;
+            ArmInputSense[R][i].read_u16(float(intt_.si)/0xffff);
+            //uint16_t intt = map(in_,13107,52428,0,65535);
+            tmp[R][4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
+            tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
+        }
+        tmp[L] = (char*)RXData;
+        signed char x1 = map8(tmp[R][1],-90,90,-100,100)+1;
+        signed char y1 = map8(tmp[R][2],-90,90,-100,100)-7;
+        signed char x2 = map8(tmp[L][1],-90,90,-100,100)+3;
+        signed char y2 = map8(tmp[L][2],-90,90,-100,100)-10;
+        //sbdbt.printf("%d %d %d %d ",x1,y1,x2,y2);
+        sbdbt.printf("sita %d ",impulse(float(y2)/100.0F));
+        char** SerialData = ;
+        bool txFlag = true;
+        switch(int i = MotionSense()){
+            case NOTINPUT:
+                //tmp[L] = (char*)RXData;
+                //SerialData = tmp;
+            break;
+            case LOADMOTION:    
+                //sbdbt.printf("LOADMOTION");
+                //tmp[L] = (char*)RXData;
+                //SerialData = tmp;
+            break;
+            case TIMEOUT:
+                //sbdbt.printf("TIMEOUT");
+                //tmp[L] = (char*)RXData;
+                //SerialData = tmp;
+            break;
+            default:
+                id = i;
+                sbdbt.printf("GET %d ",i);
+                txFlag = false;
+            break;
+        }
+        //if(playMotionFlag == true){
+            playMotion(0,(uint8_t**)SerialData);
+        //}
+        //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
+        sbdbt.printf("\n");
+        //送信データを送る
+        //SerialData = tmp;
+        if(send == true || txFlag == true){
+            for(int j = 0; j < 2 ; j++){
+                for(int i = 0 ; i < dataNum ; i++){
+                    //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
+                    //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
+                }
+            }
+            //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]);
+             for(int j = 0; j < 2 ; j++){
+                for(int i = 0 ; i < dataNum ; i++){
+                    sbdbt.printf("%3d ",SerialData[j][i]);
+                    //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
+                }
+            }
+            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(0.01);
+        #if DEBUG
+            int RL = R;
+            //if(DEBUG_R){sbdbt.printf("R:");RL = R;}
+            //if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
+            //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
+            //sbdbt.printf("\n");
+        #endif
+    }
+}
\ No newline at end of file