
改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 10:cf5c159e729a, committed 2019-03-08
- Comitter:
- yuki0701
- Date:
- Fri Mar 08 07:09:33 2019 +0000
- Parent:
- 9:63924280272d
- Commit message:
- a
Changed in this revision
--- a/can/can.cpp Thu Mar 07 05:47:18 2019 +0000 +++ b/can/can.cpp Fri Mar 08 07:09:33 2019 +0000 @@ -32,9 +32,9 @@ id1_value[6] = (msg.data[0]>>1)%2; //decide right/left(0 or 1) t1_r = msg.data[3]; //value of t(0~7) - debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r" - ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]); - debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]); + //debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r" + // ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]); + //debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]); } if(msg.id == 3) {
--- a/main.cpp Thu Mar 07 05:47:18 2019 +0000 +++ b/main.cpp Fri Mar 08 07:09:33 2019 +0000 @@ -40,53 +40,85 @@ #ifdef HARUROBO_TEST_MODE + id1_value[0] = 1; + + //while(1); + gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0); - gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0); + set_cond(2,0,-1050,1,1076); + gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0); MaxonControl(0,0,0,0); - set_cond(2,0,-1243,1,1076); pos_correction(-1672,1400,0,0,0); enc_correction(1,1); wait(0.5); gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0); purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); - purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); - gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); + purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); + gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); MaxonControl(0,0,0,0); pos_correction(-2962,4500,-90,1,1); - - set_cond(0,0,-2462,0,0); - gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); + set_cond(2,0,-2462,0,6000); + gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); MaxonControl(0,0,0,0); - pos_correction(-2850,4500,-90,0,1); - enc_correction(1,0); + pos_correction(-2850,4500,-90,0,0); + enc_correction(1,1); wait(0.5); gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); - purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); - set_cond(2,1,-3500,1,6324); - gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); + purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90); + gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90); + set_cond(2,1,-3500,1,6050); + gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90); MaxonControl(0,0,0,0); - pos_correction(-2700,6647,-90,0,0); + pos_correction(-2700,6600,-90,0,0); enc_correction(1,1); wait(0.5); - gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); - purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90); + gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90); + gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90); + purecurve(8,1,1,-1000,6550,-519,6000,9,1000,5,0.1,10,0.1,600,-180); gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180); - set_cond(2,1,1243,1,4000); + set_cond(2,1,-1000,1,4000); gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180); MaxonControl(0,0,0,0); pos_correction(-519,4500,-180,0,0); + enc_correction(1,1); MaxonControl(0,0,0,0); +//以下自動で初期位置に戻るプログラム + wait(3); + + gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180); + MaxonControl(0,0,0,0); + set_cond(2,0,0,0,7000); + pos_correction(-519,6600,-180,0,0); + enc_correction(1,1); + gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180); + purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180); + purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180); + MaxonControl(0,0,0,0); + set_cond(2,1,-3500,0,6000); + pos_correction(-2962,5500,-180,0,0); + enc_correction(1,1); + gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90); + gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0); + gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0); + MaxonControl(0,0,0,0); + set_cond(2,1,-3500,1,2000); + pos_correction(-3000,3500,0,0,0); + enc_correction(1,1); + MaxonControl(0,0,0,0); + #endif #ifdef HARUROBO_MAIN_MODE
--- a/movement/movement.cpp Thu Mar 07 05:47:18 2019 +0000 +++ b/movement/movement.cpp Fri Mar 08 07:09:33 2019 +0000 @@ -172,7 +172,7 @@ //x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標) double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離 - double D1=30,D2=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離 + double D1=30,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離(時計回りを正とする) now_angle=gyro.getAngle(); //ジャイロの値読み込み @@ -199,40 +199,59 @@ } else if(tgt_angle==90) { if((xy_type==0 || xy_type==2) && pm_typeX==0) { - info.nowX.usw = x_base - (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180)); + info.nowX.usw = x_base - (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { - info.nowX.usw = x_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180)); + info.nowX.usw = x_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); } if((xy_type==1 || xy_type==2) && pm_typeY==0) { - info.nowY.usw = y_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180)); + info.nowY.usw = y_base - (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { - info.nowY.usw = y_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180)); + info.nowY.usw = y_base + (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); } - } else if(tgt_angle==180) { + } else if(tgt_angle==180 || tgt_angle==-180) { if((xy_type==0 || xy_type==2) && pm_typeX==0) { - info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180)); + info.nowX.usw = x_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { - info.nowX.usw = x_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180)); + info.nowX.usw = x_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); } if((xy_type==1 || xy_type==2) && pm_typeY==0) { - info.nowY.usw = y_base - (usw_data1+ R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180)); + info.nowY.usw = y_base - (usw_data1+ R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { - info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180)); + info.nowY.usw = y_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); + + } + } 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)); + + } 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)); + + } + 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)); + + } 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)); } } @@ -264,16 +283,17 @@ }*/ } -void enc_correction(int x_select,int y_select){ //エンコーダの座標を超音波センサの座標で上書き +void enc_correction(int x_select,int y_select) //エンコーダの座標を超音波センサの座標で上書き +{ //x_select,y_select → (0:上書きしない/1:上書きする) - if(x_select == 1){ + if(x_select == 1) { info.nowX.enc = info.nowX.usw; } - if(y_select == 1){ + if(y_select == 1) { info.nowY.enc = info.nowY.usw; } - + } //ここからそれぞれのプログラム///////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -504,24 +524,26 @@ { //距離に比例させて補正初速度を増加させる。(最大速度を設定しそれ以上は出ないようにする) - double first_speed, first_speed50 = 200, last_speed = 50, Max_speed = 1000; + double first_speed, first_speed50 = 150, last_speed = 25, Max_speed = 500; double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ double out; calc_xy(tgt_angle, u, v); - - r = hypot(now_x - tgt_x, now_y - tgt_y); + + //r = hypot(now_x - tgt_x, now_y - tgt_y); while(1) { //機体の位置を目標領域(目標座標+許容誤差)に収める - - first_speed = first_speed50 * r / 50; - - if(first_speed > Max_speed){ + + //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); }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); + MaxonControl(0,0,0,0); calc_xy(tgt_angle, u, v); @@ -534,19 +556,21 @@ while(1) { - calc_gyro(); + //calc_gyro(); + now_angle=gyro.getAngle(); + printf("angle = %f\n\r",now_angle); out = 10 * (tgt_angle - now_angle); if(out > 300) { //0~179°のときは時計回りに回転 - MaxonControl(300,300,300,300); + MaxonControl(-300,-300,-300,-300); } else if(out < -300) { - MaxonControl(-300,-300,-300,-300); + MaxonControl(300,300,300,300); } else if(out <= 300 && out > -300) { - MaxonControl(out,out,out,out); + MaxonControl(-out,-out,-out,-out); } - if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 + if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 if(id1_value[0] != 1)break; } MaxonControl(0,0,0,0);