ハヤト タジリ
/
MAID_ROBOT_makerfair
スネーク
Diff: main.cpp
- Revision:
- 0:6c2e6a485519
- Child:
- 1:e49ca9ffcccb
--- /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