File content as of revision 11:d50a0e730654:

#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;
}