File content as of revision 0:6c2e6a485519:

#include "mbed.h"
#include "BNO055.h"
#include "Ping.h"
#include "Motor.h"

Serial pico(D1,D0);
I2C i2c(D14,D15);
Ping uss(A1);

//タイマー繰り返し割り込み
Ticker uss_time;

//ボール追従アルゴリズム
float r = 0.2; //半径の調整(0~0.7)
const float Pi = 3.14159265;

//PID関連
Timer TimePID;
double dtdeg, preTimedeg, Pdeg, Ideg, Ddeg, prePdeg,Bnodeg, A ,P;//方位PID用変数
double dtpow, preTimepow, Ppow, Ipow, Dpow, prePpow, B;//パワー用PID
const float kpdeg = 0.01;//方位センサーPID定数入力 0.005以上はあげない
const float kideg = 0.01;
const float kddeg = 0.005;
const float kppow = 1.5;//パワーPID定数
const float kipow = 0.5;
const float kdpow = 0.1;

//モーター
Motor motor(PB_4,PB_5,PB_3,PA_10,PC_7,PA_9,PA_8,PB_10);

//赤外線センサー関連
AnalogIn balldegree(A3);//0のときボール無し10~100のときボール有
float ballbase,ball,nowball,Godeg;//ボール角度用変数,進行方向
AnalogIn balldistance(A2);//0~1で距離を表示ボール無いとき
DigitalIn holdsensor(PC_3);//ホールドセンサ




//スイッチ類
DigitalIn sw_right(PD_2);//プログラム開始スイッチ//タクト
DigitalIn sw_left(PC_11);//ジャイロリセット//タクト
DigitalIn swdebug(PC_10);//デバッグモードスイッチhighでon
DigitalIn swkick(PC_12);//キーパーモードフォワードモード



//プロトタイプ宣言
//****************************************************************//
//関数名:balldeg(赤外線ボールの角度,赤外線ボールの距離)//
//返り値:進行方向
//****************************************************************//
float balldeg(float ADeg, float ADis);




//*****************************************************************//
//関数名:PIDpower(赤外線ボールの角度)
//返り値:パワーの操作量を返す
//*****************************************************************//
float PIDpower(float BDeg);//パワーの操作量を返す(引数は上と同様)




//*****************************************************************//
//関数名:PIDdegree(ジャイロの基準値)//////////////////
//返り値:方位の修正角度
//*****************************************************************//
float PIDdegree(int aimdegree);//方位修正操作量を返す


//*****************************************************************//
//関数名:talk(プリセット番号を入力)
//返り値:無し
//*****************************************************************//
void talk(int pat);




//*****************************************************************//
//関数名:call_cm(距離(整数)0~300)
//返り値:無し
//*****************************************************************//
void call_cm(int range);



//******************************************************//
//関数名:uss_check()
//返り値無し
//概要:タイマー繰り返し割り込みで使う
//*******************************************************//
int uss_check();



//BNO055
BNO055 imu(i2c,PC_8);
BNO055_ID_INF_TypeDef bno055_id_inf;
BNO055_EULER_TypeDef  euler_angles;



//その他
DigitalIn ZigIn(PA_11);//通信
DigitalOut ZigOut(PB_12);



int range;//超音波センサの距離グローバル変数


//****************************************************/////
//////////////////////プログラム主文/////////////////////////
//*****************************************************/////

int main()
{

    //***************************************************////
    /////////////////////////初期設定/////////////////////////
    //***************************************************////

    //////////モーターpwm周期設定////////////
    pico.baud(9600);
    pico.printf("?");
    motor.setPwmPeriod(0.03);

    /////////////モードチェンジIMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF////////
    imu.reset();
    imu.change_fusion_mode(MODE_IMU);
    wait(0.1);
    imu.get_Euler_Angles(&euler_angles);
    float basedegree = 0;//HMC6352read()かeuler_angles.h(ただしIMUのときは0)
    TimePID.start();
    TimePID.reset();//ジャイロリセット
    preTimedeg = TimePID.read();
    motor.omni4Wheels(0,0,0);
    /////////////////////////////////////////////////////////////////





    //************************************************************/////
    ////////////////////プログラムスタート////////////////////////////////
    ///////////////////////////////////////////////////////////////////
    //************************************************************/////
    int degreeB,range,direction;
    while(1) {
        uss.Send();
        wait_ms(40);
        range = uss.Read_cm();
        degreeB = (balldegree.read() - 0.1)*400 - 185;
        if(sw_right == 1) {
            if(balldegree.read() > 0.05) {
                if(degreeB <= -20) {
                    motor.omni4Wheels(0,100,30);
                    direction = 1;
                } 
                else if(degreeB >= 20) {
                    motor.omni4Wheels(0,100,-30);
                    direction = 2;
                } 
                else {
                    if(range <= 100) {
                        motor.omni4Wheels(0,0,0);
                        talk(13);
                    } 
                    else {
                        motor.omni4Wheels(0,100,-1*degreeB/60);
                    }
                }
            } 
            else {
                if(direction == 1){
                    motor.omni4Wheels(0,0,100);
                    }
                else if(direction == 2){
                    motor.omni4Wheels(0,0,-100);
                }
                else{
                    motor.omni4Wheels(0,0,0);
                }
            }
        } 
        else {
            if(range <= 200) {
                talk(10);
            } 
            direction = 0;
            motor.omni4Wheels(0,0,0);
        }

    }



}



//******************************************************////////
///////////進行方向の角度を返す(ボールの角度,ボールの距離)////////////
//********************************************************/////
float balldeg(float ADeg, float ADis)
{

    ball = ADeg - 185;//範囲は-180~180


    nowball = ball; //範囲は-360~360

    if(nowball> 180) { //範囲を-180~180に変換
        nowball = -1 * (360 - nowball);
    } else if(nowball< -180) {
        nowball = 360 - (-1 * nowball);
    }

    if((nowball >= -55) && (nowball <= 55)) { //ボールが前にあるとき
        return(2*ball);
    } else if((nowball <= -150) && (nowball >= 150)) {
        if(nowball >= 0) {
            Godeg = ball + 90;
            return(Godeg);
        } else {
            Godeg = ball - 90;
            return(Godeg);
        }
    } else {
        if(ADis <= r) { //ボールとの距離が近いとき(半径)
            if(nowball >= 0) {
                Godeg = ball + (180 - 180/Pi*asin(ADis / r));
                return(Godeg);
            } else {
                Godeg = nowball - (180 - 180/Pi*asin(ADis / r));
                return(Godeg);
            }
        } else {
            if(nowball >= 0) {
                Godeg = ball + 180/Pi * asin(r / ADis);//数値は半径
                return(Godeg);
            } else {
                Godeg = ball - 180/Pi * asin(r / ADis);
                return(Godeg);
            }
        }
    }
}


//**********************************************////
///////////////PIDでパワーレベルを返す/////////////////
//***********************************************///
float PIDpower(float BDeg)
{

    dtpow = TimePID.read() - preTimepow;//微小時間
    preTimepow = TimePID.read();

    Ppow = BDeg - 185; //ボールの角度
    Ipow += Ppow * dtpow; //積分
    if(Ppow < 0) {
        Ppow = Ppow*-1;
    }
    Dpow = (Ppow - prePpow)/dtpow;//微分
    prePpow = Ppow;

    B = Ppow * kppow + Ipow * kipow + Dpow * kdpow; //操作量

    if((Ppow >= -15) && (Ppow <= 15)) { //ボールが前にあるとき
        Ipow = 0;//積分初期化
        return(90);//パワー最大値
    } else if((Ppow >= 90)&&(Ppow <=-90)) {
        return(90);
    } else {
        if(B <= 60) {
            return(60);//パワー最小値
        } else if(B >= 90) {
            return(90);//パワー最大値
        } else {
            return(B);
        }
    }
}

//*****************************************************///
///////////PID目標値(角度)を代入すると操作量を返す/////////////
//*****************************************************///
float PIDdegree(int aimdegree)
{
    dtdeg = TimePID.read() - preTimedeg;//微小時間
    preTimedeg = TimePID.read();

    imu.get_Euler_Angles(&euler_angles);//bno055角度取得
    Bnodeg = euler_angles.h;
    Pdeg = Bnodeg - aimdegree;//比例bno055

    if(Pdeg > 180) {    //角度値を-180~180に修正
        Pdeg = -1 * (360 - Pdeg);
    } else if(Pdeg < -180) {
        Pdeg = 360 - (-1 * Pdeg);
    }
    if(Bnodeg > 180) {    //角度値を-180~180に修正
        Bnodeg = -1 * (360 - Bnodeg);
    } else if(Pdeg < -180) {
        Bnodeg = 360 - (-1 * Bnodeg);
    }
    Ideg += Pdeg * dtdeg;//積分
    Ddeg = (Pdeg - prePdeg)/dtdeg;//微分
    prePdeg = Pdeg;
    if((Pdeg > -2) && (Pdeg < 2)) {
        Pdeg = 0;
        Ideg = 0;
        Ddeg = 0;

    }

    A = Pdeg * kpdeg + Ideg * kideg + Ddeg * kddeg;

    if(A > 1) {
        A = 1;
    } else if(A < -1) {
        A = -1;
    }
    return(A);
}


//************************************************************************//
//関数名:talk(プリセット番号を入力)
//返り値:無し
//************************************************************************//

void talk (int pat)
{
    switch (pat) {
        case 0:

            pico.printf("hajimema'site\r");
            wait(2);
            break;
        case 1:

            pico.printf("watasiwa/meidoro'bo akemityandayo-?\r");
            wait(3);
            break;
        case 2:

            pico.printf("mataki'te;ne-?\r");
            wait(3);
            break;
        case 3:

            pico.printf("yo'-koso o-punkya'npasue\r");
            wait(3);
            break;
        case 4:

            pico.printf("yamete-?\r");
            wait(3);
            break;
        case 5:

            pico.printf("ame' do'-zo?\r");
            wait(3);
            break;
        case 6:

            pico.printf("ko'uko'useinomina'san konnitiwa\r");
            wait(3);
            break;
        case 7:

            pico.printf("wa'tasiwane? ta'jirisenpaigatuku'ttandayo'.\r");
            wait(5);
            break;
        case 8:

            pico.printf("ze'hi ;iwatekenritudaigakunikitekudasai omatisi'teimasu.\r");
            wait(3);
            break;
        case 9:

            pico.printf("ariga'to-.\r");
            wait(3);
            break;
        case 10:

            pico.printf("ohayo'-go'zaimasu.\r");
            wait(3);
            break;
        case 11:

            pico.printf("konnitiwa'\r");
            wait(3);
            break;
        case 12:

            pico.printf("kyo'umoitinitiotukaresa'ma;\r");
            wait(3);
            break;

        case 13:
            pico.printf("o\mataseshimashita gosyujinsa'ma;\r");
            wait(5);
            break;


    }
}

//*****************************************************************//
//関数名:call_cm(距離(整数)0~300)
//返り値:無し
//*****************************************************************//
void call_cm(int range)
{
    if(range>=300) {
        pico.printf("sanbyaku\r");
    } else  if(range>=200) {
        range= range-200 ;
        pico.printf("nihyaku");
    } else if (range>=100) {
        range= range-100;
        pico.printf("hyaku");
    }

    if (range>=90) {
        range= range-90;
        pico.printf("kyuujyu-");
    } else if(range>=80) {
        range= range-80;
        pico.printf("hatijyu-");
    } else if(range>=70) {
        range= range-70;
        pico.printf("nanajyu-");
    } else if(range>=60) {
        range= range-60;
        pico.printf("rokujyu-");
    } else if(range>=50) {
        range= range-50;
        pico.printf("gojyu-");
    } else if(range>=40) {
        range= range-40;
        pico.printf("yonjyu-");
    } else if(range>=30) {
        range= range-30;
        pico.printf("sanjyu-");
    } else if(range>=20) {
        range= range-20;
        pico.printf("nijyu-");
    } else if(range>=10) {
        range= range-10;
        pico.printf("jyu-");
    }

    if(range==9) {
        pico.printf("kyuu\r");
        wait(3);
    } else if(range==8) {
        pico.printf("hati\r");
        wait(3);
    } else if(range==7) {
        pico.printf("nana\r");
        wait(3);
    } else if(range==6) {
        pico.printf("roku\r");
        wait(3);
    } else if(range==5) {
        pico.printf("go\r");
        wait(3);
    } else if(range==4) {
        pico.printf("yon\r");
        wait(3);
    } else if(range==3) {
        pico.printf("san\r");
        wait(3);
    } else if(range==2) {
        pico.printf("ni\r");
        wait(3);
    } else if(range==1) {
        pico.printf("iti\r");
        wait(3);
    } else {
        pico.printf("\r");
        wait(3);
    }
}

//******************************************************//
//関数名:uss_check()
//返り値無し
//概要:タイマー繰り返し割り込みで使う
//*******************************************************//
int uss_check()
{
    int range = uss.Read_cm();
    uss.Send();
    return range;
}