Diff: main.cpp

Revision:

0:6c2e6a485519

Child:

1:0d7cc59d485e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 18 09:59:24 2018 +0000
@@ -0,0 +1,504 @@
+#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;
+}
\ No newline at end of file