ver2

Dependencies:   uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P

Files at this revision

API Documentation at this revision

Comitter:
yuki0701
Date:
Wed Dec 11 04:59:36 2019 +0000
Parent:
1:26fc1b2f1c42
Commit message:
A

Changed in this revision

EC.lib Show annotated file Show diff for this revision Revisions of this file
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
pathfollowing/PathFollowing.cpp Show annotated file Show diff for this revision Revisions of this file
pathfollowing/PathFollowing.h Show annotated file Show diff for this revision Revisions of this file
uw_28015.lib Show annotated file Show diff for this revision Revisions of this file
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 EC.lib
--- 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
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 main.cpp
--- 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
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 movement/movement.cpp
--- 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;
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 movement/movement.h
--- 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();
 
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 pathfollowing/PathFollowing.cpp
--- 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;
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 pathfollowing/PathFollowing.h
--- 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);
 //機体目標角度設定関数
 
diff -r 26fc1b2f1c42 -r 820dcd23c8e3 uw_28015.lib
--- /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