ver2
Dependencies: uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P
Revision 2:820dcd23c8e3, committed 2019-12-11
- Comitter:
- yuki0701
- Date:
- Wed Dec 11 04:59:36 2019 +0000
- Parent:
- 1:26fc1b2f1c42
- Commit message:
- A
Changed in this revision
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 EC.lib --- a/EC.lib Sat Nov 16 06:26:57 2019 +0000 +++ b/EC.lib Wed Dec 11 04:59:36 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/F3RC4/code/EC/#4bc324e21350 +https://os.mbed.com/teams/2019-11/code/EC/#bb5068ea1444
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 main.cpp --- a/main.cpp Sat Nov 16 06:26:57 2019 +0000 +++ b/main.cpp Wed Dec 11 04:59:36 2019 +0000 @@ -32,63 +32,156 @@ int main() { - - //UserLoopSetting_maxon(); - UserLoopSetting_sensor(); - UserLoopSetting_can(); -/* - #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 + + //UserLoopSetting_maxon(); + UserLoopSetting_sensor(); + UserLoopSetting_can(); + /* + #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 + + printf("ソースファイル名 : %s¥n", __FILE__); + printf("作成日付 : %s¥n", __DATE__); + printf("作成時刻 : %s¥n", __TIME__); + + #endif + */ + int move_flag = 0; + while(1) { + + id1_value[0] = 1; + switch(id1_value[0]) { + //-----auto mode----------------------------------------------------------------------------------------------------------------------// + case 1: + + + //set_condの引数詳細 + //xy_type:(0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む) + //pm_typeX:(0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む) + //x_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標) + //pm_typeY:(0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む) + //y_base:超音波センサーで読む壁の座標(x軸平行の壁のy座標) + + - printf("ソースファイル名 : %s¥n", __FILE__); - printf("作成日付 : %s¥n", __DATE__); - printf("作成時刻 : %s¥n", __TIME__); +// set_cond(2,0,0,1,0); +// int f = 0; +// while(1) { +// if(f > 20) break; +// f++; +// printf("wait\n\r"); +// } + //gogo_straight(0,0,0,0,100000,100000,200,800,5,0.1,10,0.1,250,0, 4095, 30); +// wait(100000); + + set_cond(2,0,1950,1,0); + uwflag_change(1,0,0,1); + gogo_straight(1,1,457,457,700,463,300,300,5,0.1,10,0.1,250,0, 4095, 200); + gogo_straight(0,0,700,463,1300,500,300,150,5,0.1,10,0.1,250,0, 4095, 200); + mt_stop(); + wait(0.2); + pos_correction(1450,500,0,0,0,100); + enc_correction(1,1); + wait(2); + + gogo_straight(0,0,1500,500,700,500,300,300,5,0.1,10,0.1,250,0, 4095, 200); + gogo_straight(0,0,700,500,530,500,300,100,5,0.1,10,0.1,250,0, 4095, 200); + mt_stop(); + uwflag_reset(); + wait(1000); + + + +/* spline_move(1, 1, 0, 0, 200,1000, 0,300,200,700,200, 800,5,0.1,10,0.1,500,0, 4095, 500, 10); + gogo_straight(1,1,200,1000,200,2750,800,800,5,0.1,10,0.1,250,0, 4095, 200); + purecurve(3,1,1,200,2750,-800,3500,9,800,10,0.1,10,0.1,250,-90,4095, 700); + purecurve(5,1,1,-800,3500,-1800,2750,9,800,10,0.1,10,0.1,250,-90,4095, 700); + gogo_straight(1,1,-1800,2750,-1800,1000,800,500,5,0.1,10,0.1,250,-90, 4095, 500); + set_cond(1,0,-465,1,-415); + gogo_straight(1,0,-1800,1000,-1800,300,500,100,5,0.1,10,0.1,150,-90, 4095, 50); + mt_stop(); + wait(0.2); + enc_correction(0,1); + pos_correction(-1800,0,-90,1,1,50); + enc_correction2(-1800, 0); + uwflag_reset(); - #endif - */ - int move_flag = 0; - while(1) { - - id1_value[0] = 1; - switch(id1_value[0]) { - //-----auto mode----------------------------------------------------------------------------------------------------------------------// - case 1: - - // gogo_straight(1,1,0,0,200,0,50,500,5,0.1,10,0.1,50,0); -// gogo_straight(1,1,200,0,800,0,500,500,5,0.1,10,0.1,50,0); -// gogo_straight(1,1,800,0,1000,0,500,200,5,0.1,10,0.1,50,0); -// mt_stop(); -// pos_correction(1000,0,0,1,1); -// mt_stop(); -// wait(1.5); -// gogo_straight(1,1,1000,0,800,0,200,500,5,0.1,10,0.1,50,0); -// gogo_straight(1,1,800,0,200,0,500,500,5,0.1,10,0.1,50,0); -// gogo_straight(1,1,200,0,0,0,500,200,5,0.1,10,0.1,50,0); -// mt_stop(); -// pos_correction(0,0,0,1,1); - pos_correction(50,0,0,1,1); - mt_stop(); - move_flag = 1; - break; + wait(2); + gogo_straight(1,1,-1800,0,-1800,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30); + gogo_straight(1,1,-1800,400,-1800,2750,500,500,5,0.1,10,0.1,250,-90, 4095, 200); + purecurve(2,1,1,-1800,2750,-800,3500,9,500,5,0.1,10,0.1,250,0,4095, 300); + purecurve(3,1,1,-800,3500,200,2750,9,500,5,0.1,10,0.1,250,0,4095, 300); + gogo_straight(1,1,200,2750,200,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200); + gogo_straight(1,1,200,1000,200,200,500,100,5,0.1,10,0.1,250,0, 4095, 30); + mt_stop(); + wait(0.2); + pos_correction(0,0,0,1,1,50);*/ + + + + /* gogo_straight(1,1,0,0,0,400,200,800,5,0.1,10,0.1,250,0, 4095, 30); + gogo_straight(1,1,0,400,0,3000,800,800,5,0.1,10,0.1,250,0, 4095, 200); + purecurve(3,1,1,0,3000,-1000,3750,9,800,5,0.1,10,0.1,250,-90,4095, 500); + purecurve(5,1,1,-1000,3750,-2000,3000,9,800,5,0.1,10,0.1,250,-90,4095, 500); + gogo_straight(1,1,-2000,3000,-2000,1000,800,800,5,0.1,10,0.1,250,-90, 4095, 500); + //////set_cond(2,0,-600,1,-600); + gogo_straight(1,1,-2000,1000,-2000,200,800,100,5,0.1,10,0.1,100,-90, 4095, 50); + mt_stop(); + wait(0.2); + pos_correction(-2000,0,-90,1,1,50); + enc_correction2(-2000, 0); + + wait(2); + gogo_straight(1,1,-2000,0,-2000,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30); + gogo_straight(1,1,-2000,400,-2000,3000,500,500,5,0.1,10,0.1,250,-90, 4095, 200); + purecurve(2,1,1,-2000,3000,-1000,3750,9,500,5,0.1,10,0.1,250,0,4095, 300); + purecurve(3,1,1,-1000,3750,0,3000,9,500,5,0.1,10,0.1,250,0,4095, 300); + gogo_straight(1,1,0,3000,0,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200); + gogo_straight(1,1,0,1000,0,200,500,100,5,0.1,10,0.1,250,0, 4095, 30); + mt_stop(); + wait(0.2); + pos_correction(0,0,0,1,1,50);*/ + + - //-----wait mode----------------------------------------------------------------------------------------------------------------------// - case 0: + /* gogo_straight(1,1,0,0,0,400,50,200,5,0.1,10,0.1,250,0, 4095, 30); + gogo_straight(1,1,0,400,0,2000,500,500,5,0.1,10,0.1,250,0, 4095, 100); + gogo_straight(1,1,0,2000,0,2700,500,100,5,0.1,10,0.1,250,0, 4095, 50); + mt_stop(); + wait(2); + pos_correction(0,3000,0,1,1,50); - calc_xy(0,1,1); - ashi_led(); - //MaxonControl(0,0,0,0); - go_waitmode = 0; + gogo_straight(1,1,0,3000,0,2600,50,500,5,0.1,10,0.1,0,0, 4095,30); + gogo_straight(1,1,0,2600,0,1000,500,500,5,0.1,10,0.1,100,0, 4095, 100); + gogo_straight(1,1,0,1000,0,300,500,100,5,0.1,10,0.1,0,0, 4095, 50); + mt_stop(); + wait(2); + pos_correction(0,0,0,1,1,50);*/ + + while(1) mt_stop(); + + + move_flag = 1; + break; - break; - //-----manual mode--------------------------------------------------------------------------------------------------------------------// - case 2: + //-----wait mode----------------------------------------------------------------------------------------------------------------------// + case 0: - ManualOut(250,100,500,200); - go_waitmode = 0; + calc_xy(0,1,1); + ashi_led(); + //MaxonControl(0,0,0,0); + go_waitmode = 0; - break; - } - //------------------------------------------------------------------------------------------------------------------------------------// + break; + //-----manual mode--------------------------------------------------------------------------------------------------------------------// + case 2: + + ManualOut(250,100,500,200); + go_waitmode = 0; + + break; + } + //------------------------------------------------------------------------------------------------------------------------------------// if(move_flag == 1)break; - } - + } + } \ No newline at end of file
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 movement/movement.cpp --- a/movement/movement.cpp Sat Nov 16 06:26:57 2019 +0000 +++ b/movement/movement.cpp Wed Dec 11 04:59:36 2019 +0000 @@ -8,6 +8,7 @@ #include "manual.h" #include "can.h" #include "R6093U.h" +#include "uw.h" #define PI 3.141592 @@ -20,9 +21,15 @@ int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3; Ec EC2(p16,p15,NC,2048,0.05); -Ec EC1(p18,p17,NC,500,0.05); +Ec EC1(p18,p17,NC,2048,0.05); + + +Uw uw1(p28); +Uw uw4(p27); + Ticker ec_ticker; //ec角速度計算用ticker +Ticker uw_ticker; //uw値取得用ticker //R1370P gyro(p9,p10); @@ -44,6 +51,8 @@ int RL_mode; +int uw_flag1 = 0,uw_flag2 = 0,uw_flag3 = 0,uw_flag4 = 0; + ///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言///////////////// /*「info.(機体情報の種類).(使用センサーの種類)」に各情報を格納する @@ -75,10 +84,11 @@ gyro.initialize(); ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 + uw_ticker.attach(&cal_uw,0.05); EC1.setDiameter_mm(70); EC2.setDiameter_mm(70); //測定輪半径//後で測定 - info.nowX.enc = 0; //初期位置の設定 - info.nowY.enc = 0; + info.nowX.enc = 457; //初期位置の設定 + info.nowY.enc = 457; } void UserLoopSetting_enc_right() @@ -99,6 +109,29 @@ { EC1.CalOmega(); EC2.CalOmega(); + + //usw_data1 = 10 * uw1.get_dist(); + ////usw_data2 = 10 * uw2.get_dist(); + //usw_data3 = 10 * uw3.get_dist(); + ////usw_data4 = 10 * uw4.get_dist(); +} + +void cal_uw() //uw値取得用 +{ + if(uw_flag1 == 1) { + usw_data1 = 10 * uw1.get_dist(); + //printf("uw1 = %f\n\r",usw_data1); + } + if(uw_flag2 == 1) { + //usw_data2 = 10 * uw2.get_dist(); + } + if(uw_flag3 == 1) { + //usw_data3 = 10 * uw3.get_dist(); + } + if(uw_flag4 == 1) { + usw_data4 = 10 * uw4.get_dist(); + //printf("uw4 = %f\n\r",usw_data4); + } } void output(double FL,double BL,double BR,double FR) @@ -219,8 +252,8 @@ //pm_typeX,pm_typeY:(0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む) //x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標) - double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離 - double D1=30,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離(時計回りを正とする) + double R1=414.5,R2=414.5,R3=414.5,R4=414.5; //機体の中心から各超音波センサーが付いている面までの距離 + double D1=237.5,D2=237.5,D3=237.5,D4=237.5; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離(時計回りを正とする) // now_angle=gyro.getAngle(); //ジャイロの値読み込み now_angle = -gyro.getZ_Angle(); @@ -229,19 +262,23 @@ if((xy_type==0 || xy_type==2) && pm_typeX==0) { info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180)); + uw_flag4 = 1; } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180)); + uw_flag3 = 1; } if((xy_type==1 || xy_type==2) && pm_typeY==0) { info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180)); + uw_flag2 = 1; } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180)); + uw_flag1 = 1; } @@ -249,19 +286,23 @@ if((xy_type==0 || xy_type==2) && pm_typeX==0) { info.nowX.usw = x_base - (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); + uw_flag1 = 1; } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { info.nowX.usw = x_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); + uw_flag2 = 1; } if((xy_type==1 || xy_type==2) && pm_typeY==0) { info.nowY.usw = y_base - (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); + uw_flag4 = 1; } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { info.nowY.usw = y_base + (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); + uw_flag3 = 1; } @@ -269,48 +310,79 @@ if((xy_type==0 || xy_type==2) && pm_typeX==0) { info.nowX.usw = x_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); + uw_flag3 = 1; } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { info.nowX.usw = x_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); + uw_flag4 = 1; } if((xy_type==1 || xy_type==2) && pm_typeY==0) { info.nowY.usw = y_base - (usw_data1+ R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); + uw_flag1 = 1; } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { info.nowY.usw = y_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); + uw_flag2 = 1; } } else if(tgt_angle==-90) { if((xy_type==0 || xy_type==2) && pm_typeX==0) { info.nowX.usw = x_base - (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); + uw_flag2 = 1; } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { info.nowX.usw = x_base + (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); + uw_flag1 = 1; } if((xy_type==1 || xy_type==2) && pm_typeY==0) { info.nowY.usw = y_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); + uw_flag3 = 1; } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { info.nowY.usw = y_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); + uw_flag4 = 1; } } } +void uwflag_reset() +{ + uw_flag1 = 0; + uw_flag2 = 0; + uw_flag3 = 0; + uw_flag4 = 0; +} + +void uwflag_change(int u1,int u2, int u3, int u4) +{ + uw_flag1 = u1; + uw_flag2 = u2; + uw_flag3 = u3; + uw_flag4 = u4; +} + + void calc_xy(double target_angle, double u,double v) { //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する calc_xy_enc(); + //usw_data1 = 10 * uw1.get_dist(); + ///usw_data2 = 10 * uw2.get_dist(); + //usw_data3 = 10 * uw3.get_dist(); + ///usw_data4 = 10 * uw4.get_dist(); + + //printf("uw2 = %f, uw4 = %f\n\r",usw_data2,usw_data4); if(u != 1 || v != 1) { calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。 @@ -345,6 +417,12 @@ } +void enc_correction2(int x_plot1, int y_plot2) //引数の座標でエンコーダの座標を修正 +{ + info.nowX.enc = x_plot1; + info.nowY.enc = y_plot2; +} + //ここからそれぞれのプログラム///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正) //ジャイロの出力は角度だが三角関数はラジアンとして計算する @@ -359,13 +437,14 @@ double q_p,double q_d, double r_p,double r_d, double r_out_max, - double target_angle) + double target_angle, double v_base, double q_out_max) //type:動きの種類(8パターン) point_x1,point_y1=出発地点の座標 point_x2,point_x2=目標地点の座標 theta=plotの間隔(0~90°) speed=速度 { //-----PathFollowingのパラメーター設定-----// q_setPDparam(q_p,q_d); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 set_r_out(r_out_max); //旋回時の最大出力値設定関数 + set_q_out(q_out_max); set_target_angle(target_angle); //機体目標角度設定関数 int s; @@ -482,7 +561,7 @@ CalMotorOut(x_out,y_out,r_out); //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out); - base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095); //m1~m4に代入 + base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),v_base); //m1~m4に代入 //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++; @@ -501,13 +580,14 @@ double q_p,double q_d, double r_p,double r_d, double r_out_max, - double target_angle) + double target_angle,double v_base, double q_out_max) //引数:出発地点の座標(x,y)、目標地点の座標(x,y)、初速度(speed1)、目標速度(speed2)//speed1=speed2 のとき等速運動 { //-----PathFollowingのパラメーター設定-----// q_setPDparam(q_p,q_d); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 set_r_out(r_out_max); //旋回時の最大出力値設定関数 + set_q_out(q_out_max); set_target_angle(target_angle); //機体目標角度設定関数 while (1) { @@ -523,17 +603,127 @@ CalMotorOut(x_out,y_out,r_out); //printf("out1=%lf, out2=%lf, out3=%lf, out4=%lf\n",GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); - base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095); + base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),v_base); //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m_1,m_2,m_3,m_4); // MaxonControl(m1,m2,m3,m4); // debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle); printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle); + //printf("usw2 = %f usw4 = %f x=%f y=%f angle=%f\n\r",usw_data2,usw_data4,now_x,now_y,now_angle); if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break; } } + + +double spline_base(int i, int k, double t, int nv[]) //スプライン基底関数を求める関数 +{ + // i:0~(制御点の個数-1) + // k:スプライト曲線の次元 + // t:0~(ノットベクトルの最大値) + // nv[]:ノットベクトル + double w1 = 0.0, w2 = 0.0; + if (k == 1) { + if (t > nv[i] && t <= nv[i + 1]) + return 1.0; + else + return 0.0; + } else { + if ((nv[i + k] - nv[i + 1]) != 0) { + w1 = ((nv[i + k] - t) / (nv[i + k] - nv[i + 1])) * spline_base(i + 1, k - 1, t, nv); + //printf("%f\n\r",w1); + } + if ((nv[i + k - 1] - nv[i]) != 0) { + w2 = ((t - nv[i]) / (nv[i + k - 1] - nv[i])) * spline_base(i, k - 1, t, nv); + //printf("%f\n\r",w2); + } + return (w1 + w2); + } +} + + +void spline_move(double u, double v, + double st_x,double st_y,double end_x,double end_y, + double cont1_x,double cont1_y,double cont2_x,double cont2_y, + double st_speed, double end_speed, + double q_p,double q_d, + double r_p,double r_d, + double r_out_max, + double target_angle, double v_base, double q_out_max, int num) +{ + double dx, dy, dr; + int nt[] = {0, 0, 0, 1, 2, 2, 2}; //ノットベクトル + //dr = (end_angle - st_angle) / num; + int ds = (end_speed - st_speed) / num; + + //-----PathFollowingのパラメーター設定-----// + q_setPDparam(q_p,q_d); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 + r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 + set_r_out(r_out_max); //旋回時の最大出力値設定関数 + set_q_out(q_out_max); + set_target_angle(target_angle); //機体目標角度設定関数 + + double plotx[num + 1]; //楕円にとるplotのx座標 + double ploty[num + 1]; + double value_t; + int i, j; + int t = 0; + // for(i = 0; i < 7; i++){ + // printf("not_V = %d\n\r",nt[i]); + // } + for (i = 0; i < num + 1; i++) { + plotx[i] = 0.0; + ploty[i] = 0.0; + } + printf("{\n"); + for (i = 0; i < num + 1; i++) { + value_t = (double)2 * i / num; + for (j = 0; j < 4; j++) { + if (j == 0) { + plotx[i] += st_x * spline_base(j, 3, value_t, nt); + ploty[i] += st_y * spline_base(j, 3, value_t, nt); + } else if (j == 1) { + plotx[i] += cont1_x * spline_base(j, 3, value_t, nt); + ploty[i] += cont1_y * spline_base(j, 3, value_t, nt); + } else if (j == 2) { + plotx[i] += cont2_x * spline_base(j, 3, value_t, nt); + ploty[i] += cont2_y * spline_base(j, 3, value_t, nt); + } else if (j == 3) { + plotx[i] += end_x * spline_base(j, 3, value_t, nt); + ploty[i] += end_y * spline_base(j, 3, value_t, nt); + } + } + //printf("plot_x = %f, plot_y = %f\n\r", plotx[i], ploty[i]); + } + while(1) { + + if(id1_value[0] != 1)break; + if(id1_value[6] != flag)break; + + calc_xy(target_angle,u,v); + + XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,st_speed+ds*t,st_speed+ds*(t+1)); + CalMotorOut(x_out,y_out,r_out); + //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out); + + base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),v_base); //m1~m4に代入 + //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); + + if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++; + +// MaxonControl(m1,m2,m3,m4); //出力 +// debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle); + printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle); + + if(t == num)break; + } + +} + + + + /*void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //位置補正(使用前にMaxonControl(0,0,0,0)を入れる) { @@ -574,12 +764,12 @@ MaxonControl(0,0,0,0); }*/ -void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる) +void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v, double v_base) //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる) { //距離に比例させて補正初速度を増加させる。(最大速度を設定しそれ以上は出ないようにする) - double first_speed, first_speed50 = 10, last_speed = 10, Max_speed = 500; - double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ + double first_speed, first_speed50 = 10, last_speed = 10, Max_speed = 500, speed5 = 20; + double r, R=50; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ double out; calc_xy(tgt_angle, u, v); @@ -594,13 +784,17 @@ //first_speed = first_speed50 * r / 50; /*if(first_speed > Max_speed){ - gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle); + gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle, v_base); }else{ gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed,last_speed,5,0.1,10,0.1,500,tgt_angle); }*/ //gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle); - gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,10,10,5,0.1,10,0.1,50,tgt_angle); + + int diff_sm = hypot(now_x-tgt_x,now_y-tgt_y); + + int f_speed = diff_sm / 5 * (speed5 - last_speed); + gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,f_speed,last_speed,0.5,0.05,5,0.05,20,tgt_angle, v_base, 30); //gogo_straight(1,1,0,0,200,0,50,500,5,0.1,10,0.1,50,0); //gogo_straight(u,v,now_x,now_y,0,100,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle); @@ -625,24 +819,26 @@ //calc_gyro(); // now_angle=gyro.getAngle(); now_angle = -gyro.getZ_Angle(); - printf("angle = %f\n\r",now_angle); + if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 + else if(now_angle > tgt_angle + 1)out = 5 * (tgt_angle - now_angle + 1); + else if(tgt_angle - 1 > now_angle)out = 5 * (tgt_angle - now_angle - 1); - out = 10 * (tgt_angle - now_angle); + printf("angle = %f out = %f\n\r",now_angle,out); - if(out > 300) { //0~179°のときは時計回りに回転 + if(out > 100) { //0~179°のときは時計回りに回転 // MaxonControl(-300,-300,-300,-300); - m1 = -300; - m2 = -300; - m3 = -300; - m4 = -300; + m1 = -100; + m2 = -100; + m3 = -100; + m4 = -100; - } else if(out < -300) { + } else if(out < -100) { // MaxonControl(300,300,300,300); - m1 = 300; - m2 = 300; - m3 = 300; - m4 = 300; - } else if(out <= 300 && out > -300) { + m1 = 100; + m2 = 100; + m3 = 100; + m4 = 100; + } else if(out <= 100 && out > -100) { // MaxonControl(-out,-out,-out,-out); m1 = -out; m2 = -out; @@ -650,7 +846,7 @@ m4 = -out; } - if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 + } // MaxonControl(0,0,0,0); m1 = 0;
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 movement/movement.h --- a/movement/movement.h Sat Nov 16 06:26:57 2019 +0000 +++ b/movement/movement.h Wed Dec 11 04:59:36 2019 +0000 @@ -21,6 +21,8 @@ void calOmega(); +void cal_uw(); + void output(double FL,double BL,double BR,double FR); void base(double FL,double BL,double BR,double FR,double Max); @@ -35,10 +37,16 @@ void calc_xy_usw(double tgt_angle); +void uwflag_reset(); + +void uwflag_change(int u1,int u2, int u3, int u4); + void calc_xy(double tgt_angle, double u, double v); void enc_correction(int x_select,int y_select); +void enc_correction2(int x_plot1, int y_plot2); + void purecurve(int type,double u, double v, //正面を変えずに円弧or楕円を描いて曲がる double point_x1,double point_y1, double point_x2,double point_y2, @@ -47,7 +55,7 @@ double q_p,double q_d, double r_p,double r_d, double r_out_max, - double target_angle); + double target_angle, double v_base, double q_out_max); void gogo_straight(double u, double v, double x1_point,double y1_point, //直線運動プログラム double x2_point,double y2_point, @@ -55,9 +63,20 @@ double q_p,double q_d, double r_p,double r_d, double r_out_max, - double target_angle); + double target_angle, double v_base, double q_out_max); + +double spline_base(int i, int k, double t, int nv[]); -void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v); +void spline_move(double u, double v, + double st_x,double st_y,double end_x,double end_y, + double cont1_x,double cont1_y,double cont2_x,double cont2_y, + double st_speed, double end_speed, + double q_p,double q_d, + double r_p,double r_d, + double r_out_max, + double target_angle, double v_base, double q_out_max, int num); + +void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v, double v_base); void mt_stop();
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 pathfollowing/PathFollowing.cpp --- a/pathfollowing/PathFollowing.cpp Sat Nov 16 06:26:57 2019 +0000 +++ b/pathfollowing/PathFollowing.cpp Wed Dec 11 04:59:36 2019 +0000 @@ -2,7 +2,7 @@ #include "mbed.h" #include "math.h" -double p_out,r_out_max; +double p_out,r_out_max, q_out_max; double Kvq_p,Kvq_d,Kvr_p,Kvr_d; double diff_old,diffangle,diffangle_old; double out_dutyQ,out_dutyR; @@ -37,8 +37,8 @@ out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定 diff_old=diff; - if(out_dutyQ>500)out_dutyQ=500; - if(out_dutyQ<-500)out_dutyQ=-500; + if(out_dutyQ>q_out_max)out_dutyQ=q_out_max; + if(out_dutyQ<-q_out_max)out_dutyQ=-q_out_max; old_timeQ=now_timeQ; @@ -144,6 +144,10 @@ r_out_max = r; } +void set_q_out(double q){ + q_out_max = q; +} + void set_target_angle(double t) //機体の目標角度設定関数 { target_angle = t;
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 pathfollowing/PathFollowing.h --- a/pathfollowing/PathFollowing.h Sat Nov 16 06:26:57 2019 +0000 +++ b/pathfollowing/PathFollowing.h Wed Dec 11 04:59:36 2019 +0000 @@ -27,6 +27,9 @@ void set_r_out(double r); //旋回時の最大出力値設定関数 +void set_q_out(double q); +//経路に垂直な方向の出力の最大値設定関数 + void set_target_angle(double t); //機体目標角度設定関数
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 uw_28015.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uw_28015.lib Wed Dec 11 04:59:36 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/yuki0701/code/uw_28015/#c5ad8660c8fd