Diff: robocon2017mbed_nedoControl_Lplus.cpp

Revision:

0:495b957db932

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robocon2017mbed_nedoControl_Lplus.cpp	Thu Nov 16 13:25:48 2017 +0000
@@ -0,0 +1,288 @@
+#include "mbed.h"
+#define R 0
+#define L 1
+
+#define DEBUG 0
+#define DEBUG_R 1
+#define CALIB 0
+
+#define SetupCMD 'A'
+#define Respons 'S'
+DigitalOut debugLed1(D6);
+DigitalOut debugLed2(D7);
+bool send = false;
+union floatInByte
+{
+    uint16_t si;
+    unsigned char c[2];
+};
+class AnalogInLPF : public AnalogIn
+{
+    private:
+    float alpha;
+    float prevAnalog;
+    float nowAnalog;
+    public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin)
+    {
+        alpha = alpha_;
+        prevAnalog = 0.0;
+    } 
+    float read(){
+        nowAnalog = AnalogIn::read();
+        nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
+        prevAnalog = nowAnalog;
+        return nowAnalog;
+    }
+    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);
+    }
+};
+class InLPF
+{
+    private:
+    float alpha;
+    float prevAnalog;
+    float nowAnalog;
+    public : InLPF(float alpha_ = 0.2)
+    {
+        alpha = alpha_;
+        prevAnalog = 0.0;
+    } 
+    float read(float in){
+        nowAnalog = in;
+        nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
+        prevAnalog = nowAnalog;
+        return nowAnalog;
+    }
+    short read_u16(float in){
+        nowAnalog = in;
+        nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
+        prevAnalog = nowAnalog;
+        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.30
+AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP-0.018),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)};
+InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)};
+#if !CALIB
+//1号機
+/*
+uint16_t MinimumRangeR[4] = {12800,41414,26368,11776};
+uint16_t MaxmumRangeR[4] = {36608,52242,55552,38144};
+uint16_t MinimumRangeL[4] = {23040,24832,24576,32256};
+uint16_t MaxmumRangeL[4] = {44312,47296,59392,58624};
+
+bool ReverseL[4] = {false,true,true,false};
+bool ReverseR[4] = {true,false,true,false};
+*/
+/*
+uint16_t MinimumRangeR[4] = {14592,41414,16128,12288};
+uint16_t MaxmumRangeR[4] = {41216,52242,61952,38144};
+uint16_t MinimumRangeL[4] = {23040,18176,11008,34303};
+uint16_t MaxmumRangeL[4] = {44312,47296,60106,58368};
+
+bool ReverseL[4] = {false,true,true,false};
+bool ReverseR[4] = {true,false,true,false};
+*/
+/*
+uint16_t MinimumRangeR[4] = {1280,42240,36096,17152};
+uint16_t MaxmumRangeR[4] = {45312,52992,58624,41216};
+uint16_t MinimumRangeL[4] = {23040,18432,24576,58112};
+uint16_t MaxmumRangeL[4] = {45312,58112,59392,30976};
+*/
+//2号機
+//17468 23308
+
+uint16_t MinimumRangeR[4] = {0,0,45056,13002};
+uint16_t MaxmumRangeR[4] = {0xffff,0xffff,60676,38312};
+uint16_t MinimumRangeL[4] = {0,0,10190,55588};
+uint16_t MaxmumRangeL[4] = {0xffff,0xffff,43264,30208};
+
+bool ReverseL[4] = {false,true,true,true};
+bool ReverseR[4] = {true,false,true,false};
+
+#endif
+#if CALIB
+uint16_t MinimumRangeR[4] = {0,0,0,0};
+uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff};
+uint16_t MinimumRangeL[4] = {0,0,0,0};
+uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff};
+#endif
+//uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
+//uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
+//AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)};
+Serial dev(D1,D0);
+Serial sbdbt(D13,D12);
+#define dataNum 12
+void waitTime(float ti){
+    Timer t;
+    t.start();
+    while(ti > t.read());
+    t.stop();
+    return;
+}
+Timer timer;
+char *tmp[2];
+char RXData[dataNum] = {'0'}; 
+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 < 2 ; i++){
+            RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
+            RXData[5 + i*2] = 0;//uint8_t(intt&0xff);   //マスター片腕
+        }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n"); 
+        int i=2;
+            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_,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);
+            //uint16_t intt = map(in_,13107,52428,0,65535);
+            RXData[4 + i*2] = 0;//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");
+        i = 3;
+            floatInByte in2;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
+            in2.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
+            in2.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
+            uint16_t in2_ = ArmSense2[i].read_u16(float(in2.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 intt2 = map(in2_,MinimumRangeL[i],MaxmumRangeL[i],0,43690);
+            if(intt2 > 43690)intt2 = 43690;
+            else if(intt2 > 21845)intt2 = 21845;
+            else intt2 = 0;
+            #if !CALIB
+            intt2 = ReverseL[i] == true ? 43690 - intt2 : intt2;
+            #endif
+            floatInByte intt2_;
+            intt2_.si = intt2;
+            
+            //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d  ",intt);
+            //uint16_t intt = map(in_,13107,52428,0,65535);
+            RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
+            RXData[5 + i*2] = intt2_.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);
+}
+double offset[4] = {0,0,0,-132};
+double range[4] = {120,120,90,240};
+double range2[4] = {120,120,90,9};
+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 < 10 ; i ++ ){
+        debugLed2 = !debugLed2;
+        wait(0.01);
+    }
+    wait(0.5);
+    dev.attach(RX, Serial::RxIrq);
+    dev.putc('L');
+    timer.start();        
+    int count = 0;
+    while(1) {
+        //送信データ格納
+        tmp[R][0] = 'H';    
+        tmp[R][1] = 0;
+        tmp[R][2] = 0;
+        tmp[R][3] = 0;
+
+        for(int i = 0 ;i < 4 ; i++){
+            floatInByte intt_;
+            uint16_t in = ArmSense[i].read_u16();
+            uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
+            #if !CALIB
+            intt = ReverseR[i] == true ? 0xffff - intt : intt;
+            #endif 
+            intt_.si = intt;
+            tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
+            tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff);   //マスター片腕
+        }
+        //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
+        tmp[L] = RXData;
+        char** SerialData = tmp;
+        
+        //送信データを送る
+        //SerialData = tmp;
+        if(count > 20 && send == true){
+            #if CALIB
+            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]);
+            #endif
+            for(int j = 0; j < 2 ; j++){
+                for(int i = 0 ; i < dataNum ; i++){
+                    //sbdbt.printf("%3d ",int8_t(SerialData[j][i]));
+                    #if !CALIB
+                        if(!DEBUG)sbdbt.putc(SerialData[j][i]);
+                    #endif
+                }
+            }
+            count = 0;
+        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(1.0/1000);
+        count ++;
+    }
+}
+uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
+  // check it's within the range
+  if (inMin<inMax) { 
+    if (in <= inMin) 
+      return outMin;
+    if (in >= inMax)
+      return outMax;
+  } else {  // cope with input range being backwards.
+    if (in >= inMin) 
+      return outMin;
+    if (in <= inMax)
+      return outMax;
+  }
+  // calculate how far into the range we are
+  float scale = float(in-inMin)/float(inMax-inMin);
+  // calculate the output.
+  return uint16_t(outMin + scale*float(outMax-outMin));
+}
\ No newline at end of file