![](/media/cache/profiles/e2fb52675e06ed2b9c143270ad44bdc2.jpg.50x50_q85.jpg)
Sd
Diff: main2.cpp
- Revision:
- 0:eefd142a81de
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main2.cpp Sun Jan 14 01:06:49 2018 +0000 @@ -0,0 +1,258 @@ +#if 1 +#include "mbed.h" +#include "Nunchuck.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; + 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); + } +}; +uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax); +#define LP 0.7 +AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP),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] = {4000,40000,53500,16000}; +//uint16_t MaxmumRangeR[4] = {55000,51000,58900,45000}; +//uint16_t MinimumRangeL[4] = {31500,8000,30000,19800}; +//uint16_t MaxmumRangeL[4] = {48000,46400,46200,45000}; +//2号機 +/* +uint16_t MinimumRangeR[4] = {0xffff-43000,48000,52100,0xffff-52000}; +uint16_t MaxmumRangeR[4] = {0xffff-29800,53300,56000,0xffff-28400}; +uint16_t MinimumRangeL[4] = {62900,11300,37700,14200}; +uint16_t MaxmumRangeL[4] = {45800,56000,27900,44000}; +*/ + +uint16_t MinimumRangeR[4] = {19900,17664,45056,13002}; +uint16_t MaxmumRangeR[4] = {42544,39424,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 }; +//bool ReverseL[4] = {true,false,true,true}; +//bool ReverseR[4] = {true,false,false,false}; +//AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)}; +Nunchuck ctrl(D4,D5); +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_,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); + 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); +} +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 < 50 ; i ++ ){ + debugLed2 = !debugLed2; + wait(0.1); + } + dev.attach(RX, Serial::RxIrq); + dev.putc('L'); + timer.start(); + while(1) { + //送信データ格納 + + + 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; + //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); //マスター片腕 + } + //if(DEBUG && DEBUG_R)sbdbt.printf("\n"); + tmp[L] = RXData; + char** SerialData = tmp; + + //送信データを送る + //SerialData = tmp; + if(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:"); + 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 + } +} +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)); +} +#endif \ No newline at end of file