methods.h

00001 #include "mbed.h"
00002 void RX();
00003 class AnalogInLPF : public AnalogIn
00004 {
00005     private:
00006     float alpha;
00007     float prevAnalog;
00008     float nowAnalog;
00009     public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin)
00010     {
00011         alpha = alpha_;
00012         prevAnalog = 0.0;
00013     } 
00014     float read(){
00015         nowAnalog = AnalogIn::read();
00016         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00017         prevAnalog = nowAnalog;
00018         return nowAnalog;
00019     }
00020     short read_u16(){
00021         nowAnalog = AnalogIn::read();
00022         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00023         prevAnalog = nowAnalog;
00024         return short(nowAnalog*0xFFFF);
00025     }
00026 };
00027 class InLPF
00028 {
00029     private:
00030     float alpha;
00031     float prevAnalog;
00032     float nowAnalog;
00033     public : InLPF(float alpha_ = 0.2)
00034     {
00035         alpha = alpha_;
00036         prevAnalog = 0.0;
00037     } 
00038     float read(float in){
00039         nowAnalog = in;
00040         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00041         prevAnalog = nowAnalog;
00042         return nowAnalog;
00043     }
00044     short read_u16(float in){
00045         nowAnalog = in;
00046         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00047         prevAnalog = nowAnalog;
00048         return short(nowAnalog*0xFFFF);
00049     }
00050 };
00051 class InHPF
00052 {
00053     private:
00054     float alpha;
00055     float prevAnalog;
00056     float nowAnalog;
00057     public:float originAnalog;
00058     public : InHPF(float alpha_ = 0.2)
00059     {
00060         alpha = alpha_;
00061         prevAnalog = 0.0;
00062     } 
00063     float read(float in){
00064         originAnalog = in;
00065         nowAnalog = originAnalog;
00066         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00067         prevAnalog = nowAnalog;
00068         return originAnalog - nowAnalog;
00069     }
00070     short read_u16(float in){
00071         originAnalog = in;
00072         nowAnalog = originAnalog;
00073         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00074         prevAnalog = nowAnalog;
00075         return short(originAnalog*0xffff - nowAnalog*0xFFFF);
00076     }
00077     float read(){
00078         return originAnalog - nowAnalog;
00079     }
00080     short read_u16(){
00081         return short(originAnalog*0xFFFF - nowAnalog*0xFFFF);
00082     }
00083     
00084 };
00085 class AccHPF
00086 {
00087     private:
00088     float alpha;
00089     float prevAnalog;
00090     float nowAnalog;
00091     public:float originAnalog;
00092     public : AccHPF(float alpha_ = 0.2)
00093     {
00094         alpha = alpha_;
00095         prevAnalog = 0.0;
00096     }
00097     float read(float in){
00098         originAnalog = in;
00099         nowAnalog = originAnalog;
00100         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00101         prevAnalog = nowAnalog;
00102         return originAnalog - nowAnalog;
00103     }
00104     float read(){
00105         return originAnalog - nowAnalog;
00106     }
00107 };
00108 #define LP 1
00109 AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)};
00110 InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)};
00111 
00112 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)}};
00113 AccHPF accSense[2][3] = {{AccHPF(0.2),AccHPF(0.2),AccHPF(0.2)} , {AccHPF(0.2),AccHPF(0.2),AccHPF(0.2)}};
00114 int8_t map8(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax) {
00115             // check it's within the range
00116             if (inMin<inMax) {
00117                 if (in <= inMin)
00118                     return outMin;
00119                 if (in >= inMax)
00120                     return outMax;
00121             } else { // cope with input range being backwards.
00122                 if (in >= inMin)
00123                     return outMin;
00124                 if (in <= inMax)
00125                     return outMax;
00126             }
00127             // calculate how far into the range we are
00128             float scale = float(in-inMin)/float(inMax-inMin);
00129             // calculate the output.
00130             return int8_t(outMin + scale*float(outMax-outMin));
00131         }
00132 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
00133   // check it's within the range
00134   if (inMin<inMax) { 
00135     if (in <= inMin) 
00136       return outMin;
00137     if (in >= inMax)
00138       return outMax;
00139   } else {  // cope with input range being backwards.
00140     if (in >= inMin) 
00141       return outMin;
00142     if (in <= inMax)
00143       return outMax;
00144   }
00145   // calculate how far into the range we are
00146   float scale = float(in-inMin)/float(inMax-inMin);
00147   // calculate the output.
00148   return uint16_t(outMin + scale*float(outMax-outMin));
00149 }
00150 
00151 void waitTime(float ti){
00152     Timer t;
00153     t.start();
00154     while(ti > t.read());
00155     t.stop();
00156     return;
00157 }
00158 int8_t getLAccx(){
00159     return (int8_t)RXData[12];
00160 }
00161 int8_t getLAccy(){
00162     return (int8_t)RXData[13];
00163 }
00164 int8_t getLAccz(){
00165     return (int8_t)RXData[14];
00166 }
00167 int8_t getRAccx(){
00168     return (int8_t)ctrl.accx();
00169 }
00170 int8_t getRAccy(){
00171     return (int8_t)ctrl.accy();
00172 }
00173 int8_t getRAccz(){
00174     return (int8_t)ctrl.accz();
00175 }
00176 int8_t (*getAcc[2][3])(void) = {{getRAccx,getRAccy,getRAccz},{getLAccx,getLAccy,getLAccz}};
00177 bool motionTriggerFlag = false;
00178 int motionCount[motionNum] = {0};
00179 Timer motionTimer;
00180 int loopCount = 0;
00181 void initMotion(){
00182     motionTimer.reset();
00183     loopCount = 0;
00184     for(int i = 0 ; i < motionNum ; i++)motionCount[i] = 0;
00185     motionTriggerFlag = false;
00186         
00187 }
00188 int MotionSense(){
00189     charInBool data;
00190     charInBool data1;
00191     //加速度データを取得する。　
00192     for(int i = 0 ; i < 2 ; i ++ ){
00193         for(int j = 0 ; j < 3; j++){
00194             accData[i][j] = getAcc[i][j]();
00195             accSense[i][j].read(float(accData[i][j])/0xff);
00196         }
00197     }
00198     //モーション認識中にモーション入力がないときTIMEOUTを返す。
00199     if(motionTimer.read() > 0.2F && motionTriggerFlag == true){
00200         initMotion();
00201         return TIMEOUT;
00202     }
00203     //登録されたモーションと類似しているモーションIDを返す。
00204     #define max(a, b) ((a) > (b) ? (a) : (b))
00205     if(loopCount == 10){
00206         int max_motionCount=0;
00207         int ID = NOTINPUT;
00208         for(int i = 0 ; i < motionNum ; i ++){
00209             max_motionCount = max(max_motionCount,motionCount[i]);
00210             if(max_motionCount == motionCount[i])ID = i;
00211         }
00212         initMotion();
00213         return ID;
00214     }
00215     
00216     //加速度が一定の強さを越えるとモーション認識を開始する。
00217     if(motionTriggerFlag == false){
00218         bool f = false;
00219         for(int i = 0 ; i < 2 ; i ++ ){
00220             for(int j = 0 ; j < 3; j++){
00221                 f = f | bool(accSense[i][j].read() > 0.3F);
00222             }
00223         }
00224     
00225         if(!f)return NOTINPUT;
00226         motionTriggerFlag = true;
00227         for(int i = 0 ; i < motionNum ; i++)motionCount[i] = 0;
00228     }
00229     //可変抵抗の値を取得
00230     for(int i = 0; i < 2; i++ ){
00231         for (int j = 0 ; j < 4 ; j ++){
00232             data.data = (data.data<<1)|bool(fabs(ArmInputSense[i][j].read()) > 0.1F);
00233             data1.data = (data1.data<<1)|bool(ArmInputSense[i][j].originAnalog > 0.5F);
00234         }
00235     }
00236     /*sbdbt.printf("\n");
00237     for(int i = 7 ; i >= 0 ; i--)
00238         sbdbt.printf("%d",(data.data>>i)&1);
00239     sbdbt.printf("\n");
00240     for(int i = 7 ; i >= 0 ; i--)
00241         sbdbt.printf("%d",(data1.data>>i)&1);
00242     sbdbt.printf("\n");*/
00243     //可変抵抗に動きがあるかを判定
00244     if(data.data != 0){
00245         //可変抵抗の値と全モーションデータの比較
00246         for(int i = 0 ; i < motionNum ; i++){
00247             char d = data1.data & motion[i][loopCount];
00248             for(int j = 0 ; j < 8 ; j++){
00249                 motionCount[i] += bool((d>>j)&1);
00250             }
00251             //等しければモーションカウントを++
00252             sbdbt.printf("motionCount[%d] %d",i,motionCount[i]);
00253         }
00254         //全体ループを++
00255         loopCount ++;
00256         //タイマーリセット
00257         motionTimer.reset();
00258         //モーション認識中なのでLOADMOTIONを返す。
00259         return LOADMOTION; 
00260     }
00261     return NOTINPUT;
00262 }
00263 int playMotionCount = 0;
00264 bool playMotionFlag = false;
00265 void playMotion(int id,uint8_t** copy){
00266     if(playMotionFlag == false && playMotionCount == 0 ){
00267         playMotionFlag = true;
00268     }
00269     
00270     for(int j = 0; j < 2 ; j++){
00271         for(int i = 0 ; i < dataNum-3 ; i++){
00272             for(int s = 0; s < 2; s++)
00273             {
00274                for(int m = 0 ; m < 12 ; m ++)copy[s][m] = PlayMotion[id][playMotionCount][s][m];
00275             }
00276             
00277         }
00278     }
00279    //         if(!DEBUG)sbdbt.printf("\n");
00280    //         dev.putc('L');
00281    //         waitTime(0.01);
00282     //    }
00283    // }
00284     playMotionCount ++;
00285     if(playMotionCount == playMotionFlame){
00286         playMotionCount = 0;
00287         playMotionFlag = false;
00288     }
00289 }
00290 float prevR=0;
00291 int impulseCount = 0;
00292 int impulseFlag = false;
00293 bool impulse(double y){
00294     //if(sieta1 > PI/3 && sieta1 < PI*2/3.0)
00295     float r = y;
00296     float temp = r;
00297     r = r*0.1F + (1-0.1F)*prevR;
00298     prevR = r;
00299     //sbdbt.printf("  %f",temp - r);
00300     if(impulseFlag == true)impulseCount++;
00301     if((temp - r) > 0.6F && impulseFlag == false){
00302         impulseFlag = true;
00303     }
00304     if((temp - r) < -0.1F && impulseCount <= 5 && impulseFlag == true){
00305         impulseCount = 0;
00306         impulseFlag = false;
00307         return true;
00308     }
00309     if(impulseCount > 6 && impulseFlag == true){
00310         impulseCount = 0;
00311         impulseFlag = false;
00312     }
00313     return false;
00314 }