うい
Fork of MyLib by
Diff: Methods/methods.h
- Revision:
- 11:d50a0e730654
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Methods/methods.h Wed Oct 25 10:57:13 2017 +0000 @@ -0,0 +1,314 @@ +#include "mbed.h" +void RX(); +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); + } +}; +class InHPF +{ + private: + float alpha; + float prevAnalog; + float nowAnalog; + public:float originAnalog; + public : InHPF(float alpha_ = 0.2) + { + alpha = alpha_; + prevAnalog = 0.0; + } + float read(float in){ + originAnalog = in; + nowAnalog = originAnalog; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return originAnalog - nowAnalog; + } + short read_u16(float in){ + originAnalog = in; + nowAnalog = originAnalog; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return short(originAnalog*0xffff - nowAnalog*0xFFFF); + } + float read(){ + return originAnalog - nowAnalog; + } + short read_u16(){ + return short(originAnalog*0xFFFF - nowAnalog*0xFFFF); + } + +}; +class AccHPF +{ + private: + float alpha; + float prevAnalog; + float nowAnalog; + public:float originAnalog; + public : AccHPF(float alpha_ = 0.2) + { + alpha = alpha_; + prevAnalog = 0.0; + } + float read(float in){ + originAnalog = in; + nowAnalog = originAnalog; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return originAnalog - nowAnalog; + } + float read(){ + return originAnalog - nowAnalog; + } +}; +#define LP 1 +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)}; + +InHPF ArmInputSense[2][4] = {{InHPF(0.2),InHPF(0.2),InHPF(0.2),InHPF(0.2)} , {InHPF(0.01),InHPF(0.005),InHPF(0.01),InHPF(0.01)}}; +AccHPF accSense[2][3] = {{AccHPF(0.2),AccHPF(0.2),AccHPF(0.2)} , {AccHPF(0.2),AccHPF(0.2),AccHPF(0.2)}}; +int8_t map8(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_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 int8_t(outMin + scale*float(outMax-outMin)); + } +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)); +} + +void waitTime(float ti){ + Timer t; + t.start(); + while(ti > t.read()); + t.stop(); + return; +} +int8_t getLAccx(){ + return (int8_t)RXData[12]; +} +int8_t getLAccy(){ + return (int8_t)RXData[13]; +} +int8_t getLAccz(){ + return (int8_t)RXData[14]; +} +int8_t getRAccx(){ + return (int8_t)ctrl.accx(); +} +int8_t getRAccy(){ + return (int8_t)ctrl.accy(); +} +int8_t getRAccz(){ + return (int8_t)ctrl.accz(); +} +int8_t (*getAcc[2][3])(void) = {{getRAccx,getRAccy,getRAccz},{getLAccx,getLAccy,getLAccz}}; +bool motionTriggerFlag = false; +int motionCount[motionNum] = {0}; +Timer motionTimer; +int loopCount = 0; +void initMotion(){ + motionTimer.reset(); + loopCount = 0; + for(int i = 0 ; i < motionNum ; i++)motionCount[i] = 0; + motionTriggerFlag = false; + +} +int MotionSense(){ + charInBool data; + charInBool data1; + //加速度データを取得する。 + for(int i = 0 ; i < 2 ; i ++ ){ + for(int j = 0 ; j < 3; j++){ + accData[i][j] = getAcc[i][j](); + accSense[i][j].read(float(accData[i][j])/0xff); + } + } + //モーション認識中にモーション入力がないときTIMEOUTを返す。 + if(motionTimer.read() > 0.2F && motionTriggerFlag == true){ + initMotion(); + return TIMEOUT; + } + //登録されたモーションと類似しているモーションIDを返す。 + #define max(a, b) ((a) > (b) ? (a) : (b)) + if(loopCount == 10){ + int max_motionCount=0; + int ID = NOTINPUT; + for(int i = 0 ; i < motionNum ; i ++){ + max_motionCount = max(max_motionCount,motionCount[i]); + if(max_motionCount == motionCount[i])ID = i; + } + initMotion(); + return ID; + } + + //加速度が一定の強さを越えるとモーション認識を開始する。 + if(motionTriggerFlag == false){ + bool f = false; + for(int i = 0 ; i < 2 ; i ++ ){ + for(int j = 0 ; j < 3; j++){ + f = f | bool(accSense[i][j].read() > 0.3F); + } + } + + if(!f)return NOTINPUT; + motionTriggerFlag = true; + for(int i = 0 ; i < motionNum ; i++)motionCount[i] = 0; + } + //可変抵抗の値を取得 + for(int i = 0; i < 2; i++ ){ + for (int j = 0 ; j < 4 ; j ++){ + data.data = (data.data<<1)|bool(fabs(ArmInputSense[i][j].read()) > 0.1F); + data1.data = (data1.data<<1)|bool(ArmInputSense[i][j].originAnalog > 0.5F); + } + } + /*sbdbt.printf("\n"); + for(int i = 7 ; i >= 0 ; i--) + sbdbt.printf("%d",(data.data>>i)&1); + sbdbt.printf("\n"); + for(int i = 7 ; i >= 0 ; i--) + sbdbt.printf("%d",(data1.data>>i)&1); + sbdbt.printf("\n");*/ + //可変抵抗に動きがあるかを判定 + if(data.data != 0){ + //可変抵抗の値と全モーションデータの比較 + for(int i = 0 ; i < motionNum ; i++){ + char d = data1.data & motion[i][loopCount]; + for(int j = 0 ; j < 8 ; j++){ + motionCount[i] += bool((d>>j)&1); + } + //等しければモーションカウントを++ + sbdbt.printf("motionCount[%d] %d",i,motionCount[i]); + } + //全体ループを++ + loopCount ++; + //タイマーリセット + motionTimer.reset(); + //モーション認識中なのでLOADMOTIONを返す。 + return LOADMOTION; + } + return NOTINPUT; +} +int playMotionCount = 0; +bool playMotionFlag = false; +void playMotion(int id,uint8_t** copy){ + if(playMotionFlag == false && playMotionCount == 0 ){ + playMotionFlag = true; + } + + for(int j = 0; j < 2 ; j++){ + for(int i = 0 ; i < dataNum-3 ; i++){ + for(int s = 0; s < 2; s++) + { + for(int m = 0 ; m < 12 ; m ++)copy[s][m] = PlayMotion[id][playMotionCount][s][m]; + } + + } + } + // if(!DEBUG)sbdbt.printf("\n"); + // dev.putc('L'); + // waitTime(0.01); + // } + // } + playMotionCount ++; + if(playMotionCount == playMotionFlame){ + playMotionCount = 0; + playMotionFlag = false; + } +} +float prevR=0; +int impulseCount = 0; +int impulseFlag = false; +bool impulse(double y){ + //if(sieta1 > PI/3 && sieta1 < PI*2/3.0) + float r = y; + float temp = r; + r = r*0.1F + (1-0.1F)*prevR; + prevR = r; + //sbdbt.printf(" %f",temp - r); + if(impulseFlag == true)impulseCount++; + if((temp - r) > 0.6F && impulseFlag == false){ + impulseFlag = true; + } + if((temp - r) < -0.1F && impulseCount <= 5 && impulseFlag == true){ + impulseCount = 0; + impulseFlag = false; + return true; + } + if(impulseCount > 6 && impulseFlag == true){ + impulseCount = 0; + impulseFlag = false; + } + return false; +} \ No newline at end of file