File content as of revision 1:c26a135080b1:

/*
 *  改良しました
 *  
 *
 *
 *  何かわからないことがあったらその都度聞いて
 */

#include "mbed.h"
#include "gyro.h"
#include "ikarashiMDC.h"
#include "R1370.h"
#include "controller.h"
#define PI 3.141593

Controller pad(PC_10, PC_11, 208);
R1370 R1370(PB_6,PA_10);
Serial pc(USBTX, USBRX, 115200);
Serial serial(PC_6, PC_7, 115200);
DigitalOut serialcontrol(D10);

gyro omni(4); //オムニホイールの数を入力

/*ロボットの使うMDCに応じて変える事*/
ikarashiMDC ikarashi[] = {
    ikarashiMDC(&serialcontrol,2,0,SM,&serial),
    ikarashiMDC(&serialcontrol,2,1,SM,&serial),
    ikarashiMDC(&serialcontrol,2,2,SM,&serial),
    ikarashiMDC(&serialcontrol,2,3,SM,&serial)
};

int main()
{
    int b[11];//自作コントローラー関連
    double stickRad[2],stickNorn[2],raded[2],Norned[2];

    bool b7 = 1,b9 = 1;//b[7]とb[9]のボタン判断

    double acc[4] = {0,0,0,0},angle,control,con,func,gain = 0;//accはアクセラレート：加速の略　angleはジャイロセンサの値を入れる  control,conは角度調整に必要
    double idealAngle = 0,puls = 0.1;//funcは台形制御に必要 gainは台形制御の計算に使う idealAngleは角度セットに使う

    ikarashi[0].braking = true;//必須
/*タイヤ角を設定  必須
omni.setRad(タイヤの番号,タイヤのついてる角度radian値)
このプログラムでは4輪の設定をしている*/    
    for(int i = 0; i < 4; i++)
    {
        omni.setRad(i, PI * (i*2+1) / 4);
    }

    omni.setIdeal(0);//目的の角度を0度に設定   必須

    omni.setErrorRange(2);//誤差範囲を設定 +-入力値分の誤差が出る ロボットが止まらない時は大きめに設定すること

    omni.limitPower(0.5);//回転速度制限　ここで入力した速さよりも回転は速くならない 正の実数で１より大きくしないこと 必須

    while(1)
    {
        if(R1370.update() == 0 && pad.receiveState() == 1)//ジャイロセンサが動いてかつコントローラーと通信していれば
        {        
            angle = R1370.getAngle();//ジャイロの出した角度を保存 必須

            for(int i = 0; i < 13; i++){
                b[i] = pad.getButton1(i);//0 = 左下 1 = 左上 2 = 十字左 3 = 上 4 = 右 5 = 下 6 = 右下 7 = L2 8 = L1 9 = R2 10 = R1 11 R3 12 = L3
            }
            for(int i = 0; i < 2; i++){
                stickRad[i] = PI - pad.getRadian(i);
                stickNorn[i] = pad.getNorm(i);
            }
            pc.printf("%d %d\n\r",b[7],b[9]);
/*一度ボタンを押すとその方向に90度回転*/            
            if(b[7] != b7)
            {
                if(b7 == 1)
                {
                    idealAngle -= 90;
                    gain = 0;
                }
                b7 = b[7];
            }
            if(b[9] != b9)
            {
                if(b9 == 1)
                {
                    idealAngle += 90;
                    gain = 0;
                }
                b9 = b[9];
            }
            if(idealAngle > 360)
            {
                idealAngle = 90;
            }
            if(idealAngle <= 0)
            {
                idealAngle = 360;
            }
            omni.setIdeal(idealAngle - 180);

//omni.cAngle(ジャイロの値)
//ジャイロの角度を渡す　調整用の速度をもらう
            control = omni.cAngle(angle);
//omni.pAngle(ジャイロの値,比例ゲイン) こっちは比例制御
            con = omni.pAngle(angle,2);
/*比例ゲインは１が等倍速、２が倍速で、0.5が1/2倍速といった感じに速度が変わる
ちょうどいい比例ゲインは試しながら見つける事
この二つはどちらか一方のみを使うこと
どっちも使ってみてから決めてほしい*/

            if(fabs(raded[0] - stickRad[0]) > 2 || fabs(raded[1] - stickRad[1]) > 2 || fabs(Norned[0] - stickNorn[0]) > 0.2 || fabs(Norned[1] - stickNorn[1] > 0.2) )
            {
                gain = 0;//もしスティックが大きく動いたらgain = 0
            }

            func = omni.lFunc(0.25,gain);//一次関数の値を取り出す omni.lFunc(比例定数,gain)

            raded[0] = stickRad[0];  //値比較用
            raded[1] = stickRad[1];  //
            Norned[0] = stickNorn[0];//
            Norned[1] = stickNorn[1];//

/* スティックが傾いていないまたはどちらも傾いていると平行移動しない*/            
            if( ((stickNorn[0] == 0) && (stickNorn[1] == 0)) || ((stickNorn[0] != 0) && (stickNorn[1] != 0)) )
            {
                for(int i = 0; i < 4; i++)
                {
                    acc[i] = (omni.hMove(10,i) + con) * func;
                }//+controlまたは +conで角度がずれたら修正 この時は比例制御を使った//omni.hMove(角度radian,タイヤ番号)　で速度を返す  角度が10なら平行移動しない  必須
                gain += puls;//どんどんモーターの値が大きくなる

            }
            else if(stickNorn[0] != 0 && stickNorn[1] == 0) //stick[0]だけを動かしているとき
            {
                for(int i = 0; i < 4; i++)
                {
                    acc[i] = (omni.hMove(stickRad[0], i) * stickNorn[0] + con) * func;  //方向がロボットの向きによって変化する
                }
                gain += puls;//どんどん（ry
            }

            else //stick[1]だけを動かしているとき
            {
                for(int i = 0; i < 4; i++)
                {
                    acc[i] = (omni.hMove(stickRad[1] + (angle * (PI / 180)), i) * stickNorn[1] + con) * func;//ロボットの向きに左右されずに動く
                }
             gain += puls;//どんどん（ry
            }

            for(int i = 0; i < 4;i++) //モーターに出力 必須
            {
                if(acc[i] > 0.9)
                {
                    acc[i] = 0.9;
                }
                else if(acc[i] < -0.9)
                {
                    acc[i] = -0.9;
                }
                ikarashi[i].setSpeed(acc[i]);
                pc.printf("|%f| ",acc[i]);
            }
        }

        else//ジャイロ,コントローラーが動いていなかったら
        {
            pc.printf("error\n\r");   //errorを表示して動かない

            for(int i = 0; i < 4;i++)
            {
                ikarashi[i].setSpeed(0);//モーターに0 必須
            }
        }                         
    }
}