3/13 13時24分 up

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

Files at this revision

API Documentation at this revision

Comitter:
yuki0701
Date:
Wed Mar 13 04:23:18 2019 +0000
Parent:
9:7667dcfc7ce5
Commit message:
a

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
movement/movement.cpp Show annotated file Show diff for this revision Revisions of this file
movement/movement.h Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Wed Mar 13 03:24:49 2019 +0000
+++ b/main.cpp	Wed Mar 13 04:23:18 2019 +0000
@@ -46,6 +46,101 @@
                     case 0:
                         //-----right mode-------------------------------------------------------------------------------------------------------------//
                         flag = 0;
+                        UserLoopSetting_enc_right();
+                        if(T1 == 0) {  //スタート位置からみかんの木まで移動
+                            gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0);
+                            purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0);
+                            purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0);
+                            set_cond(2,1,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);
+                            pos_correction(1672,1400,0,0,0);
+                            enc_correction(1,1);
+                            wait(0.5);
+                            T1++;
+                        }
+                        if(T1 == 1) {
+                            while(1) {
+                                wait(0.1);
+                                if(T1 == 2) {
+                                    break;
+                                }
+                            }
+                        }
+                        if(T1 == 2) {  //みかんの木から三宝置き場まで移動
+                            gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0);
+                            purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0);
+                            purecurve(1,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(2,1,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,0);
+                            enc_correction(1,1);
+                            wait(0.5);
+                            T1++;
+                        }
+                        if(T1 == 3) {
+                            while(1) {
+                                wait(0.1);
+                                if(T1 == 4) {
+                                    break;
+                                }
+                            }
+                        }
+                        if(T1 == 4) {  //三宝置き場からりんごの木まで移動
+                            gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90);
+                            purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90);
+                            purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90);
+                            purecurve(2,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,0,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,6600,90,0,0);
+                            enc_correction(1,1);
+                            wait(0.5);
+                            T1++;
+                        }
+                        if(T1 == 5) {
+                            while(1) {
+                                wait(0.1);
+                                if(T1 == 6) {
+                                    break;
+                                }
+                            }
+                        }
+                        if(T1 == 6) {  //りんごの木からお供え台まで移動
+                            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(5,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,0,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);
+                            T1++;
+                        }
+                        if(T1 == 7) {
+                            while(1) {
+                                printf("ashi finished\n\r");
+                                MaxonControl(0,0,0,0);
+                                if(id1_value[0] != 1)break;  //これらは他のwhileにも入れる必要あり
+                                if(id1_value[6] != flag)break;
+                            }
+                        }
+                        break;
+                        
+                    case 1:
+                        //-----left mode--------------------------------------------------------------------------------------------------------------//
+                        flag = 1;
+                        UserLoopSetting_enc_left();
                         if(T1 == 0) {  //スタート位置からみかんの木まで移動
                             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);
@@ -134,10 +229,6 @@
                                 if(id1_value[6] != flag)break;
                             }
                         }
-                        break;
-                    case 1:
-                        //-----left mode--------------------------------------------------------------------------------------------------------------//
-                        flag = 1;
 
                         break;
                 }
--- a/movement/movement.cpp	Wed Mar 13 03:24:49 2019 +0000
+++ b/movement/movement.cpp	Wed Mar 13 04:23:18 2019 +0000
@@ -68,7 +68,15 @@
     ec_ticker.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
     EC1.setDiameter_mm(25.5);
     EC2.setDiameter_mm(25.5);  //測定輪半径//後で測定
-    info.nowX.enc = -2962; //初期位置の設定
+}
+void UserLoopSetting_enc_right()
+{
+    info.nowX.enc = 2962; //エンコーダの初期位置の設定(右側フィールド)
+    info.nowY.enc = 3500;
+}
+void UserLoopSetting_enc_left()
+{
+    info.nowX.enc = -2962; //エンコーダの初期位置の設定(左側フィールド)
     info.nowY.enc = 3500;
 }
 
@@ -528,30 +536,36 @@
 
 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 = 150, last_speed = 25, Max_speed = 500;
+    double first_speed, first_speed50 = 100,last_speed = 50, Max_speed = 300; 
+    //first_speed50:5センチのズレを補正するときの補正初速度 / last_speed:目標速度 / first_speed:first_speed50とlast_speedを元に計算した実際の補正初速度 / Max_speed:補正速度の上限
     double r, R=10;  // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
     double out;
+    double tgt_xx, tgt_yy;
 
     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) { //機体の位置を目標領域(目標座標+許容誤差)に収める
 
-        if(id1_value[0] != 1)break;
-        if(id1_value[6] != flag)break;
+        first_speed = (first_speed50 - last_speed) * r / 50 + last_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);
+        if(r < 20){ //目標地点までの距離が1センチ以上2センチ未満のとき
+            gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,30,30,5,0.1,10,0.1,500,tgt_angle);
+        }else if(r < 30){ ////目標地点までの距離が2センチ以上3センチ未満のとき
+            gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,40,40,5,0.1,10,0.1,500,tgt_angle);
+        }else if(first_speed > Max_speed){
+            tgt_xx = ((Max_speed - last_speed)*now_x + (first_speed - Max_speed)*tgt_x)/(first_speed-last_speed);
+            tgt_yy = ((Max_speed - last_speed)*now_y + (first_speed - Max_speed)*tgt_y)/(first_speed-last_speed);
+            gogo_straight(u,v,now_x,now_y,tgt_xx,tgt_yy,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle);
+            gogo_straight(u,v,tgt_xx,tgt_yy,tgt_x,tgt_y,Max_speed,last_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);
+            //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);
 
@@ -560,13 +574,11 @@
         r=hypot(now_x - tgt_x, now_y - tgt_y);
 
         if(r < R) break;
+        if(id1_value[0] != 1)break;
     }
 
     while(1) {
 
-        if(id1_value[0] != 1)break;
-        if(id1_value[6] != flag)break;
-
         //calc_gyro();
         now_angle=gyro.getAngle();
         printf("angle = %f\n\r",now_angle);
@@ -582,6 +594,12 @@
         }
 
         if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+        if(id1_value[0] != 1)break;
     }
     MaxonControl(0,0,0,0);
-}
\ No newline at end of file
+}
+
+/*----------------------コメント------------------------*/
+/* 3/10(日):pos_correctionの中身を一部修正しました。(引数に変更はありません。)
+
+/*---------------------------------------------------*/
\ No newline at end of file
--- a/movement/movement.h	Wed Mar 13 03:24:49 2019 +0000
+++ b/movement/movement.h	Wed Mar 13 04:23:18 2019 +0000
@@ -9,6 +9,10 @@
 
 void UserLoopSetting_sensor();
 
+void UserLoopSetting_enc_right();
+
+void UserLoopSetting_enc_left();
+
 void calOmega();
 
 void output(double FL,double BL,double BR,double FR);