Diff: main.cpp

Revision:

2:5d86b09dd7d8

Parent:

0:84122dbed53a

Child:

3:21fb6b67bb54

--- a/main.cpp	Fri Oct 13 09:36:49 2017 +0000
+++ b/main.cpp	Thu Oct 19 08:41:21 2017 +0000
@@ -37,6 +37,7 @@
     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);
     }
@@ -62,26 +63,26 @@
         nowAnalog = in;
         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
         prevAnalog = nowAnalog;
-        return short(nowAnalog*0xFFFF);
+        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.5
-AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)};
+#define LP 0.30
+AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP-0.018),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP-0.018),AnalogInLPF(A3,LP)};
 InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)};
 #if !CALIB
 //1号機
-uint16_t MinimumRangeR[4] = {25700,39300,16400,53000};
-uint16_t MaxmumRangeR[4] = {44000,52600,62300,27600};
+/*
+uint16_t MinimumRangeR[4] = {25700,39300,21760,53000};
+uint16_t MaxmumRangeR[4] = {44000,52600,54270,27600};
 uint16_t MinimumRangeL[4] = {20600,28500,2114,33300};
 uint16_t MaxmumRangeL[4] = {44000,43300,58900,6021};
 //2号機
-/*
-uint16_t MinimumRangeR[4] = {11900,32600,52100,0xffff-37800};
-uint16_t MaxmumRangeR[4] = {54500,48000,56000,0xffff-8900};
-uint16_t MinimumRangeL[4] = {62900,11300,37700,14200};
-uint16_t MaxmumRangeL[4] = {45800,56000,27900,44000};
 */
+uint16_t MinimumRangeR[4] = {19900,38656,36096,17152};
+uint16_t MaxmumRangeR[4] = {43260,53760,58624,41216};
+uint16_t MinimumRangeL[4] = {21500,19400,8448,57088};
+uint16_t MaxmumRangeL[4] = {43520,38900,38400,30208};
 #endif
 #if CALIB
 uint16_t MinimumRangeR[4] = {0,0,0,0};
@@ -92,7 +93,7 @@
 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
 bool ReverseL[4] = {false,true,true,true};
-bool ReverseR[4] = {true,false,true,true};
+bool ReverseR[4] = {true,false,true,false};
 //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)};
 Nunchuck ctrl(D4,D5);
 Serial dev(D1,D0);
@@ -121,8 +122,11 @@
             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_,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);
@@ -176,8 +180,11 @@
             //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, ReverseR[i] == true ? 0xffff - MaxmumRangeR[i] : MinimumRangeR[i],ReverseR[i] == true ? 0xffff - MinimumRangeR[i] : MaxmumRangeR[i],0,65535);
+            //uint16_t intt = map(in, ReverseR[i] == true ? 0xffff - MaxmumRangeR[i] : MinimumRangeR[i],ReverseR[i] == true ? 0xffff - MinimumRangeR[i] : MaxmumRangeR[i],0,65535);
+            uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
+            #if !CALIB
             intt = ReverseR[i] == true ? 0xffff - intt : intt;
+            #endif 
             intt_.si = intt;
             //uint16_t intt = map(in_,13107,52428,0,65535);
             tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
@@ -189,14 +196,15 @@
         
         //送信データを送る
         //SerialData = tmp;
-        if(count > 10 && send == true){
-            for(int j = 0; j < 2 ; j++){
+        if(count > 20 && send == 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:");
+            }*/
+            /*
+            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]);
@@ -219,7 +227,7 @@
             waitTime(0.1);
         }
         debugLed1 = false;
-        waitTime(0.001);
+        waitTime(0.0005);
         count ++;
         #if DEBUG
             int RL = R;