ハヤト タジリ
/
MAID_ROBOT_makerfair
スネーク
main.cpp
- Committer:
- tajiri1999
- Date:
- 2018-10-18
- Revision:
- 0:6c2e6a485519
- Child:
- 1:e49ca9ffcccb
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; }