ハヤト タジリ
/
MAID_ROBOT_makerfair
スネーク
Diff: main.cpp
- Revision:
- 1:e49ca9ffcccb
- Parent:
- 0:6c2e6a485519
- Child:
- 2:e1fedc61e5e3
diff -r 6c2e6a485519 -r e49ca9ffcccb main.cpp --- a/main.cpp Thu Oct 18 09:59:24 2018 +0000 +++ b/main.cpp Mon Mar 23 11:30:19 2020 +0000 @@ -1,504 +1,216 @@ #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); +//constant +#define PI 3.1415926 -//赤外線センサー関連 -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); - - - +//Skps +Serial ps2(A0,A1,9600);//Tx,Rx(D8,D2) -//*****************************************************************// -//関数名:PIDpower(赤外線ボールの角度) -//返り値:パワーの操作量を返す -//*****************************************************************// -float PIDpower(float BDeg);//パワーの操作量を返す(引数は上と同様) - - - - -//*****************************************************************// -//関数名:PIDdegree(ジャイロの基準値)////////////////// -//返り値:方位の修正角度 -//*****************************************************************// -float PIDdegree(int aimdegree);//方位修正操作量を返す - - -//*****************************************************************// -//関数名:talk(プリセット番号を入力) -//返り値:無し -//*****************************************************************// -void talk(int pat); - - - - -//*****************************************************************// -//関数名:call_cm(距離(整数)0~300) -//返り値:無し -//*****************************************************************// -void call_cm(int range); +//SKPS_Button_number +enum { + //return value:digital + p_select, + p_joyl, + p_joyr, + p_start, + p_up, + p_right, + p_down, + p_left, + p_l2, + p_r2, + p_l1, + p_r1, + p_triangle, + p_circle, + p_cross, + p_square, + //return_value:analog + p_joy_lx, + p_joy_ly, + p_joy_rx, + p_joy_ry, + p_joy_lu, + p_joy_ld, + p_joy_ll, + p_joy_lr, + p_joy_ru, + p_joy_rd, + p_joy_rl, + p_joy_rr, + p_con_status, + //return_value:none + p_motor1, + p_motor2 +}; +//TerminalDisplay(TeraTerm) +Serial pc(SERIAL_TX, SERIAL_RX); -//******************************************************// -//関数名:uss_check() -//返り値無し -//概要:タイマー繰り返し割り込みで使う -//*******************************************************// -int uss_check(); +//voltage +AnalogIn voltage(PC_4); +//UltraSonicSensor +DigitalIn change(NC);//pa6 +Timer timer_volt; +//Motor +Motor motor(PA_12,PA_11,PA_5, PB_2,PB_1,PA_9, PB_15,PB_14,PB_13, PA_8,PB_4,PB_10); -//BNO055 -BNO055 imu(i2c,PC_8); -BNO055_ID_INF_TypeDef bno055_id_inf; -BNO055_EULER_TypeDef euler_angles; +//kicker +DigitalOut kick(NC);//pa_8 - +//ToggleSwitch +DigitalIn sw_start(PD_2); //program start switch +DigitalIn sw_reset(PC_12); //gyro sensor reset switch +DigitalIn sw_kick(USER_BUTTON); -//その他 -DigitalIn ZigIn(PA_11);//通信 -DigitalOut ZigOut(PB_12); - - - -int range;//超音波センサの距離グローバル変数 - - -//****************************************************///// -//////////////////////プログラム主文///////////////////////// -//*****************************************************///// +//declear prototype (function list) +bool check_voltage(); //電圧をチェックする +void uart_send(unsigned char data); +unsigned char uart_rec(void); +unsigned char skps(unsigned char data); +unsigned char skps_vibrate(unsigned char motor, unsigned char value); +float convert_degree(float x,float y); +float convert_length(float x,float y); +/*****************************************************************/ +/**********************main function******************************/ +/*****************************************************************/ int main() { - //***************************************************//// - /////////////////////////初期設定///////////////////////// - //***************************************************//// - - //////////モーターpwm周期設定//////////// - pico.baud(9600); - pico.printf("?"); - motor.setPwmPeriod(0.03); +//**************************************************************// +////////////////////////initialize setting//////////////////////// +//**************************************************************// + wait_ms(50); + kick = 0; //キックを解除する - /////////////モードチェンジ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); - ///////////////////////////////////////////////////////////////// - - - - + /*motor pwm frequency set*/ + motor.setPwmPeriod(0.00052); - //************************************************************///// - ////////////////////プログラムスタート//////////////////////////////// - /////////////////////////////////////////////////////////////////// - //************************************************************///// - 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); - } + while (1) { +//***************************************************************// +////////////////Play mode//////////// +//***************************************************************// + timer_volt.reset(); + timer_volt.start(); + while (sw_start == 0 && check_voltage() == 1) { + float deg,power,yaw_angle,lx,ly,rx; + + //convert int to float + lx = (float)skps(p_joy_lx); + ly = (float)skps(p_joy_ly); + rx = (float)skps(p_joy_rx); + deg = convert_degree(lx,ly); + + yaw_angle = (rx - 128)/128; + + power = convert_length(lx,ly); + if(power <= 10) { //遊びをカット + power = 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); + + if(skps(p_l1) == 0 || skps(p_square) == 0){ + motor.omniWheels(deg,power,yaw_angle*30); } - } else { - if(nowball >= 0) { - Godeg = ball + 180/Pi * asin(r / ADis);//数値は半径 - return(Godeg); - } else { - Godeg = ball - 180/Pi * asin(r / ADis); - return(Godeg); + else{ + motor.omniWheels(deg,power*0.5,yaw_angle*30); + } + /*kick*/ + if(skps(p_circle) == 0 || skps(p_r1)== 0){ + kick = 1; + wait_ms(100); + kick = 0; } } + motor.omniWheels(0, 0, 0); + /**********************end main function**************************/ } } -//**********************************************//// -///////////////PIDでパワーレベルを返す///////////////// -//***********************************************/// -float PIDpower(float BDeg) +////////////////////////////////////// +/*Battery check voltage*/ +////////////////////////////////////// +bool check_voltage() { - - 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; + static float sum[5] = { 8, 8, 8, 8, 8 }; + static float Ave = 8; + static bool S = 1; + if (timer_volt.read() > 0.2) { + for (int i = 4; i > 0; i--) { + sum[i] = sum[i - 1]; + } + sum[0] = voltage.read() * 9.9; + Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5; + if (Ave < 7.0) { //7.0V以下で自動遮断 + S = 0; + } else { + S = 1; - 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); } + timer_volt.reset(); } + return S; } -//*****************************************************/// -///////////PID目標値(角度)を代入すると操作量を返す///////////// -//*****************************************************/// -float PIDdegree(int aimdegree) +void uart_send(unsigned char data) { - dtdeg = TimePID.read() - preTimedeg;//微小時間 - preTimedeg = TimePID.read(); - - imu.get_Euler_Angles(&euler_angles);//bno055角度取得 - Bnodeg = euler_angles.h; - Pdeg = Bnodeg - aimdegree;//比例bno055 + ps2.putc(data); +} - 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); +unsigned char uart_rec(void) +{ + unsigned char temp; + while(ps2.readable() == 0); + temp = ps2.getc(); + return temp; } - -//************************************************************************// -//関数名:talk(プリセット番号を入力) -//返り値:無し -//************************************************************************// - -void talk (int pat) +unsigned char skps(unsigned char data) { - 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: + uart_send(data); + return uart_rec(); +} - 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; - - - } +unsigned char skps_vibrate(unsigned char motor, unsigned char value) +{ + uart_send(motor); + uart_send(value); } -//*****************************************************************// -//関数名: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); - } +/**************************************************************/ +/*関数名:convert_degree(float x,float y)*/ +/*引数:ジョイスティックのX,Y Axis*/ +/*戻り値:float型 極座標の角度(前方が0度,時計回りに360度)*/ +/*概要:ロボットの進行方向を決めるときに使う*/ +/*************************************************************/ +float convert_degree(float x,float y){ + float degree; + degree = (180/PI)*atan2(y - 128,x - 128) + 360 + 90;//オイラー角度に変換 + + //角度を0~360度に変換 + if(degree >= 360.0){ + degree = degree - 360; + } + return degree; } -//******************************************************// -//関数名:uss_check() -//返り値無し -//概要:タイマー繰り返し割り込みで使う -//*******************************************************// -int uss_check() -{ - int range = uss.Read_cm(); - uss.Send(); - return range; +/***************************************************************/ +/*関数名:convert_length(float x,flaot y)*/ +/*引数:ジョイスティックのX,Y*/ +/*戻り値:float型 極座標の原点から(x,y)点までの長さを0~100で返す*/ +/*概要:モーターのパワーを決めるときに,極座標の長さを使う*/ +/***************************************************************/ +float convert_length(float x,float y){ + float xy_length; + xy_length = pow(x - 128,2) + pow(y - 128,2); + xy_length = (100 * xy_length)/(128*128); + if(xy_length >= 100.0){ + xy_length = 100.0; + } + return xy_length; } \ No newline at end of file