Revision 9:7667dcfc7ce5, committed 2019-03-13

Comitter:

la00noix

Date:

Wed Mar 13 03:24:49 2019 +0000

Parent:

8:2ba338b4590e

Child:

10:c741191360de

Commit message:

a

Changed in this revision

--- a/can/can.cpp	Thu Mar 07 02:54:55 2019 +0000
+++ b/can/can.cpp	Wed Mar 13 03:24:49 2019 +0000
@@ -5,11 +5,11 @@
 #include "manual.h"
 #include "can.h"
 
-CAN can1(p30,p29,1000000);
+CAN can1(p30,p29);
 Ticker can_ticker;  //can用ticker
 
-DigitalOut cansend_led(LED1);  //canread -> on
-DigitalOut canread_led(LED2);  //cansend -> on
+DigitalOut cansend_led(LED1);  //cansend -> on
+DigitalOut canread_led(LED2);  //canread -> on
 
 int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
 
@@ -32,8 +32,8 @@
             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("[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]);
         }
 
@@ -49,6 +49,7 @@
 
             usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
             //debug_printf("usw_data4 = %f\n\r",usw_data4);
+            //debug_printf("usw1=%f usw2=%f usw3=%f usw4=%f\n\r",usw_data1,usw_data2,usw_data3,usw_data4);
         }
 
     } else {

--- a/main.cpp	Thu Mar 07 02:54:55 2019 +0000
+++ b/main.cpp	Wed Mar 13 03:24:49 2019 +0000
@@ -16,8 +16,8 @@
 int go_waitmode = 0;
 
 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
-//DigitalOut cansend_led(LED1);  //canread -> on                //can.cpp
-//DigitalOut canread_led(LED2);  //cansend -> on                //can.cpp
+//DigitalOut cansend_led(LED1);  //cansend -> on                //can.cpp
+//DigitalOut canread_led(LED2);  //canread -> on                //can.cpp
 //DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp
 
 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
@@ -45,58 +45,99 @@
                 switch(id1_value[6]) {
                     case 0:
                         //-----right mode-------------------------------------------------------------------------------------------------------------//
-                        if(T1 == 0) {
-
-                            //スタート位置からみかんの木まで移動
+                        flag = 0;
+                        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);
+                            purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,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);
+                            pos_correction(-1672,1400,0,0,0);
+                            enc_correction(1,1);
                             wait(0.5);
                             T1++;
-
                         }
                         if(T1 == 1) {
                             while(1) {
-                                wait(0.5);
+                                wait(0.1);
                                 if(T1 == 2) {
                                     break;
                                 }
                             }
                         }
-                        if(T1 == 2) {
-
-                            //みかんの木から三宝置き場まで移動
+                        if(T1 == 2) {  //みかんの木から三宝置き場まで移動
+                            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); //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,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,0);
+                            enc_correction(1,1);
+                            wait(0.5);
                             T1++;
                         }
                         if(T1 == 3) {
                             while(1) {
-                                wait(0.5);
+                                wait(0.1);
                                 if(T1 == 4) {
                                     break;
                                 }
                             }
                         }
-                        if(T1 == 4) {
-
-                            //三宝置き場からりんごの木まで移動
+                        if(T1 == 4) {  //三宝置き場からりんごの木まで移動
+                            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,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,6600,-90,0,0);
+                            enc_correction(1,1);
                             wait(0.5);
                             T1++;
                         }
                         if(T1 == 5) {
                             while(1) {
-
-                                wait(0.5);
+                                wait(0.1);
                                 if(T1 == 6) {
                                     break;
                                 }
                             }
                         }
-                        if(T1 == 6) {
-
-                            //りんごの木からお供え台まで移動
-                            wait(0.5);
+                        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(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,-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;
 
                         break;
                 }

--- a/movement/movement.cpp	Thu Mar 07 02:54:55 2019 +0000
+++ b/movement/movement.cpp	Wed Mar 13 03:24:49 2019 +0000
@@ -33,6 +33,8 @@
 
 double xy_type,pm_typeX,pm_typeY,x_base,y_base;
 
+int flag;
+
 ///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言/////////////////
 
 /*「info.(機体情報の種類).(使用センサーの種類)」に各情報を格納する
@@ -172,7 +174,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 +201,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,6 +285,19 @@
     }*/
 }
 
+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とは限らない))(反時計回りが正)
 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
@@ -392,6 +426,9 @@
 
     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,speed,speed);
@@ -407,7 +444,6 @@
         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);
 
         if(t == (90/theta))break;
-        if(id1_value[0] != 1)break;
     }
 }
 
@@ -429,6 +465,9 @@
 
     while (1) {
 
+        if(id1_value[0] != 1)break;
+        if(id1_value[6] != flag)break;
+
         calc_xy(target_angle,u,v);
 
         XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2);
@@ -444,11 +483,10 @@
         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);
 
         if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
-        if(id1_value[0] != 1)break;
     }
 }
 
-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 +524,64 @@
         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 = 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);
+
+    while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
+
+        if(id1_value[0] != 1)break;
+        if(id1_value[6] != flag)break;
+
+        //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);
+
+        r=hypot(now_x - tgt_x, now_y - tgt_y);
+
+        if(r < R) 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);
+
+        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 - 1 < now_angle && now_angle < tgt_angle + 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	Wed Mar 13 03:24:49 2019 +0000
@@ -5,6 +5,8 @@
 
 extern int16_t m1,m2,m3,m4;
 
+extern int flag;
+
 void UserLoopSetting_sensor();
 
 void calOmega();
@@ -25,6 +27,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,