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

--- 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

--- 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

--- 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;

--- 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();
 

--- 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;

--- 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);
 //機体目標角度設定関数
 

--- /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