春ロボ1班(元F3RC4班+)
改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 9:63924280272d, committed 2019-03-07
- Comitter:
- yuki0701
- Date:
- Thu Mar 07 05:47:18 2019 +0000
- Parent:
- 8:2ba338b4590e
- Commit message:
- a
Changed in this revision
--- a/main.cpp Thu Mar 07 02:54:55 2019 +0000
+++ b/main.cpp Thu Mar 07 05:47:18 2019 +0000
@@ -12,6 +12,8 @@
#define PI 3.141592
//#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
+#define HARUROBO_TEST_MODE
+//#define HARUROBO_MAIN_MODE
int go_waitmode = 0;
@@ -36,6 +38,58 @@
#endif
+#ifdef HARUROBO_TEST_MODE
+
+ 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);
+ 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);
+ 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);
+ MaxonControl(0,0,0,0);
+ pos_correction(-2850,4500,-90,0,1);
+ enc_correction(1,0);
+ 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);
+ MaxonControl(0,0,0,0);
+ pos_correction(-2700,6647,-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,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
+ set_cond(2,1,1243,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);
+ MaxonControl(0,0,0,0);
+
+#endif
+
+#ifdef HARUROBO_MAIN_MODE
while(1) {
switch(id1_value[0]) {
@@ -119,4 +173,7 @@
}
//------------------------------------------------------------------------------------------------------------------------------------//
}
+
+#endif
+
}
\ No newline at end of file
--- a/movement/movement.cpp Thu Mar 07 02:54:55 2019 +0000
+++ b/movement/movement.cpp Thu Mar 07 05:47:18 2019 +0000
@@ -264,6 +264,18 @@
}*/
}
+void enc_correction(int x_select,int y_select){ //エンコーダの座標を超音波センサの座標で上書き
+//x_select,y_select → (0:上書きしない/1:上書きする)
+
+ if(x_select == 1){
+ info.nowX.enc = info.nowX.usw;
+ }
+ if(y_select == 1){
+ info.nowY.enc = info.nowY.usw;
+ }
+
+}
+
//ここからそれぞれのプログラム/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
//ジャイロの出力は角度だが三角関数はラジアンとして計算する
@@ -448,7 +460,7 @@
}
}
-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) //位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
{
double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
@@ -486,4 +498,56 @@
if(id1_value[0] != 1)break;
}
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)を入れる)
+{
+//距離に比例させて補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする）
+
+ double first_speed, first_speed50 = 200, last_speed = 50, Max_speed = 1000;
+ double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
+ double out;
+
+ calc_xy(tgt_angle, u, v);
+
+ r = hypot(now_x - tgt_x, now_y - tgt_y);
+
+ while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
+
+ 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);
+ }
+
+ MaxonControl(0,0,0,0);
+
+ calc_xy(tgt_angle, u, v);
+
+ r=hypot(now_x - tgt_x, now_y - tgt_y);
+
+ if(r < R) break;
+ if(id1_value[0] != 1)break;
+ }
+
+ while(1) {
+
+ calc_gyro();
+
+ out = 10 * (tgt_angle - now_angle);
+
+ if(out > 300) { //0～179°のときは時計回りに回転
+ MaxonControl(300,300,300,300);
+ } else if(out < -300) {
+ MaxonControl(-300,-300,-300,-300);
+ } else if(out <= 300 && out > -300) {
+ MaxonControl(out,out,out,out);
+ }
+
+ if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+ if(id1_value[0] != 1)break;
+ }
+ MaxonControl(0,0,0,0);
}
\ No newline at end of file
--- a/movement/movement.h Thu Mar 07 02:54:55 2019 +0000
+++ b/movement/movement.h Thu Mar 07 05:47:18 2019 +0000
@@ -25,6 +25,8 @@
void calc_xy(double tgt_angle, double u, double v);
+void enc_correction(int x_select,int y_select);
+
void purecurve(int type,double u, double v, //正面を変えずに円弧or楕円を描いて曲がる
double point_x1,double point_y1,
double point_x2,double point_y2,