File content as of revision 30:00a513858a44:

//2021/10/23 紐掴むだけ

#include "mbed.h"
 
#include "jy901.h"
#include "Servo.h"
#include "QEI.h"
#include "IR2302.h"
 
#include "pin_config.h"
#include "control_parameter.h"
 
#include "lpf.h"
#include "beep.h"

Serial pc(USBTX,USBRX,115200);

Beep buzzer(BEEP);
JY901 jy(SDA, SCL);
//DigitalOut stop(STOP);

QEI encoder(QEI1 ,QEI2 ,NC ,100 ,QEI::X4_ENCODING);

lpf yAcc(LPF_C_TIME, LPF_A_TIME);
lpf zAcc(LPF_C_TIME, LPF_A_TIME);
lpf mSp(MOT_C_TIME, MOT_A_TIME);
Servo servo[3] = {SERVO1, SERVO2, SERVO3};
IR2302 ir2302(MD_A,MD_B);//梅沢追加　IR2302
Timer setWait,nextWait,jumpWait;
AnalogIn ph[3] = {PHOTO1,PHOTO2,PHOTO3};
DigitalOut light[3] = {ROPE1,ROPE2,ROPE3};
DigitalIn ub(USER_BUTTON);
DigitalIn ab(PA_15);

int main()
{
    int i=0,j=0,k=3,l=0;//0,0,2,0
    double acc[3];     //加速度
    double abac[3]={0};//初期座標軸での加速度
    double angle[3];
    float lpfY,lpfZ;  //ローパス後の加速度
    float lpfM;       //モータのローパス
    jy.calibrateAll(50);
 
    int nowPalse;
    
    float phI[3] = {0};//フォトインタラプタ
    bool rope = 0;

    bool setFlag   = 1;//1
    bool firstFlag = 0;//0
    bool secondFlag= 0;//0
    bool thirdFlag = 0;//0
    bool swingFlag = 1;//1
    bool flyFlag   = 0;//0
    bool catchFlag = 0;//0
    int8_t way     = 1;//1or-1
    bool resetFlag = 0;//0
    float fiveYAcc[5]={0};
    
    float servoAngle[3];
    float motor=0;
    
    for(int i=0;i < 3; i++)servo[i].calibrate(0.00095, 90);
    servoAngle[0] = 0.2;
    servoAngle[1] = 0.2;
    servoAngle[2] = 0.8;
    for(i=0;i<3;i++) servo[i].write(servoAngle[i]);
    for(i=0; i<3; i++) light[i] = true;
    
    ub.mode(PullDown);
    
    while(1) 
    {
//センシング
/*
    //jy901
        acc[1] = jy.getYaxisAcceleration()*10;
        acc[2] = jy.getZaxisAcceleration()*10;
        angle[0] = jy.getXaxisAngle();
        angle[1] = jy.getYaxisAngle();
        angle[2] = jy.getZaxisAngle();
        for(i=0; i<3; i++)
        {
            if(acc[i] > 1567)acc[i] -= 3134;
        }
        abac[1] = acc[1]*cos(angle[0]/180.0*PI) - acc[2] * sin(angle[0]/180.0*PI);
        abac[2] = acc[2]*cos(angle[0]/180.0*PI) + acc[1] * sin(angle[0]/180.0*PI);
        
        //ローパスフィルタ
        lpfY = yAcc.path_value(abac[1]);
        lpfZ = zAcc.path_value(abac[2]);

        //swingで使うやつ
        fiveYAcc[j%5]= lpfY;
        j++;
*/
    //ロータリーエンコーダ 向き変ったので-1掛け
        nowPalse = encoder.getPulses() * -1;
        
    //紐センサ
        for(i=0; i<3; i++) phI[i]=ph[i];
        
//処理部

    //セッティング
        if(setFlag)
        {
            if(!ub && !ab) motor=0.25;
            else if(ab && ub)  motor=-0.25;
            else if(!ab && ub) motor=0;
            else if(!ub && ab)
            {
                setFlag = 0;
                firstFlag = 1;
                buzzer.beep(3000,1.0);
                wait(5);
                buzzer.nobeep();
            }

        }

    //加速1段目
        if(firstFlag)
        {
            motor = FIRST_POWER * way * swingFlag;                 //振り子を振る、wayで反転を制御
            motor = mSp.path_value(motor);
            if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
            {
                way *= -1;
                nextWait.start();
                setWait.start();
                swingFlag = 0;
                if(nextWait > SECOND_LINE)                        //一定時間経過したらsecondに移行
                {
                    firstFlag   = 0;
                    secondFlag  = 1;
                    swingFlag = 0;
                    nextWait.stop();
                    nextWait.reset();
                }
            }
            if(setWait > FIRST_WAIT)
            {
                setWait.stop();
                setWait.reset();
                swingFlag = 1;
            }
        }

    //加速2段目
        if(secondFlag)
        {
            motor = SECOND_POWER * way * swingFlag;                 //振り子を振る、wayで反転を制御
            nextWait.start();
            if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
            {
                way *= -1;
                setWait.start();
                swingFlag = 0;
                if(nextWait > THIRD_LINE)                        //一定時間経過したらthirdに移行
                {
                    secondFlag   = 0;
                    thirdFlag  = 1;
                    swingFlag = 0;
                    nextWait.stop();
                    nextWait.reset();
                }
            }
            if(setWait > SECOND_WAIT)
            {
                setWait.stop();
                setWait.reset();
                swingFlag = 1;
            }
        }

    //加速3段目        
        if(thirdFlag)
        {
            jumpWait.start();
            motor = THIRD_POWER * way * swingFlag;                 //振り子を振る、wayで反転を制御
            if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら（少し間をおいて）反転
            {
                way *= -1;
                nextWait.start();
                setWait.start();
                swingFlag = 0;
            }
            if(setWait > THIRD_WAIT)
            {
                setWait.stop();
                setWait.reset();
                swingFlag = 1;
            }
            if((jumpWait > JUMP_TIME) && (nowPalse < JUMP_LINE) && (nowPalse > JUMP_LINE-10) && (way == -1))             //一定時間たったら前に飛ぶ
            {
                flyFlag = 1;
                thirdFlag = 0;
            }
        }


    //ジャンプ
        if(flyFlag)
        {
//            if(lpfZ < RELEASE_LINE)
            {
                servoAngle[(k-1)%3] = 0.1;//サーボ離す
                servoAngle[k%3] = 0.7;
//                l++;
                l=1;
                flyFlag = 0;
                catchFlag=1;
            }
            motor=0;
            
        }


    //キャッチ
        if(catchFlag)
        {
            if(phI[k%3] < ROPE_ACRROS[k%3])
            {
                catchFlag=0;
                motor = 0;
                resetFlag=1;
                k++;
            }
            
        }

    //掴んだ後
        if(resetFlag)
        {
            if(nowPalse < NEXT_PALSE * l)
            {
                motor=0.35;
            }
            else
            {
                motor=0;
                resetFlag = 0;
//                firstFlag  = 1;
            }
        }

 //実験用記述-----------
/* 
        if(flyFlag)
        {
//            if(lpfZ < RELEASELINE)
            {
                for(i=0; i < 3; i++) servoAngle[i] = -1.0;//サーボ離す
//                k++;
                flyFlag = 0;
            }
            motor=0;
        }
*/
/*
        for(i=0; i<3; i++) 
        {
            if(phI[i] < ROPE_ACRROS[i]) servoAngle[i] = -0.8;
            
        }   
*/
/*
        if(phI[k%3] < ROPE_ACRROS[k%3])
        {
            servoAngle[k%3] = 0.85;
            servoAngle[(k+1)%3] = 0.4;
            k++;
        }
*/
//        if(!b) motor=0.3;
//        else motor=0;

 //-------------------
 
//駆動系
 
    //サーボ
        for(i=0;i<3;i++) servo[i].write(servoAngle[i]);
    
    //モーター
        ir2302.SetMotorSpeed(motor);
      
//表示系
//        pc.printf("%.2f,%.2f,%.2f",angle[0],angle[1],angle[2]);
        pc.printf("%.2f,%.2f,%.2f,",servoAngle[0],servoAngle[1],servoAngle[2]);
        pc.printf("%.4f,",motor);
//        pc.printf("%.2f,%.2f,",abac[1],abac[2]);
//        pc.printf("%.2f,%.2f,",lpfY,lpfZ);
//        pc.printf("%d,",nowPalse);
        pc.printf("%d,",setFlag);
//        pc.printf("%d,",resetFlag);
//        pc.printf("%.2f,",lpfM);
//        pc.printf("%.3f,%.3f,%.3f,",phI[0],phI[1],phI[2]);
//        pc.printf("%d,",k);
        pc.printf("\r\n");

    }
}