File content as of revision 0:dc3a10b60a56:

#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] = {19900,41216,45056,13002};
uint16_t MaxmumRangeR[4] = {42544,53248,60676,38312};
uint16_t MinimumRangeL[4] = {21500,19432,10190,55588};
uint16_t MaxmumRangeL[4] = {43520,43300,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 < 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_,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");
            
        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));
}