Revision 6:26724c287387, committed 2019-03-02

Comitter:

la00noix

Date:

Sat Mar 02 07:48:18 2019 +0000

Parent:

5:6cebe1c458a9

Child:

7:44ce34007499

Commit message:

a

Changed in this revision

--- a/can/can.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/can/can.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -20,14 +20,16 @@
 
         if(msg.id == 1) { //from main
 
-            id1_value[0] = msg.data[0];  //decide mode(1~3)
-            id1_value[1] = msg.data[1];  //angle of left joystick(0~359)
+            id1_value[0] = (msg.data[0]>>6)%4;  //decide wait/auto/manual(0~2)
+            id1_value[1] = msg.data[1];         //angle of left joystick(0~359)
             id1_value[2] = msg.data[2];
-            id1_value[3] = msg.data[3];  //BOTTONR1 off/on(0 or 1)
-            id1_value[4] = msg.data[4];  //state of right joystick(1~3)
-            id1_value[5] = msg.data[5];  //left joystick neutral position(0 or 1)
+            id1_value[3] = (msg.data[0]>>5)%2;  //BOTTONR1 off/on(0 or 1)
+            id1_value[4] = (msg.data[0]>>3)%4;  //state of right joystick(1~3)
+            id1_value[5] = (msg.data[0]>>2)%2;  //left joystick neutral position(0 or 1)
+            id1_value[6] = (msg.data[0]>>1)%2;  //decide right/left(0 or 1)
 
-            //debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d\n\r",id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5]);
+            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]);
         }
 
         if(msg.id == 3) {
@@ -48,7 +50,7 @@
         canread_led = 0;
     }
 
-    if(can1.write(CANMessage(7,ashi_data,4))) {  //IDを7にして送信
+    if(can1.write(CANMessage(7,can_ashileddata,1))) {  //IDを7にして送信
         cansend_led = 1;
     } else {
         cansend_led = 0;
@@ -72,7 +74,6 @@
 
 void UserLoopSetting_can()
 {
-
     can1.frequency(1000000);
     can_ticker.attach(&can_readsend,0.01);  //遅かったら早める
 }
\ No newline at end of file

--- a/main.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/main.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -13,143 +13,114 @@
 
 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
 
-#define HARUROBO_TEST_MODE //テスト自動プログラム（練習・動作確認用）使用時に定義
-
 int go_waitmode = 0;
 
 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
-//DigitalOut canread_led(LED1);  //canread -> on                //can.cpp
-//DigitalOut cansend_led(LED2);  //cansend -> on                //can.cpp
+//DigitalOut cansend_led(LED1);  //canread -> on                //can.cpp
+//DigitalOut canread_led(LED2);  //cansend -> on                //can.cpp
 //DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp
 
 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
 
-int main(){
-    
+int main()
+{
     UserLoopSetting_maxon();
     UserLoopSetting_sensor();
     UserLoopSetting_can();
-      
+
 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
 
-      printf("ソースファイル名 : %s¥n", __FILE__);
-      printf("作成日付 : %s¥n", __DATE__);
-      printf("作成時刻 : %s¥n", __TIME__);
-  
+    printf("ソースファイル名 : %s¥n", __FILE__);
+    printf("作成日付 : %s¥n", __DATE__);
+    printf("作成時刻 : %s¥n", __TIME__);
+
 #endif
 
-while(1){
-    
-    id1_value[0] = 1;
-    
-    switch(id1_value[0]){
-            
+    while(1) {
+
+        switch(id1_value[0]) {
 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
-        case 1:
+            case 1:
+
+                switch(id1_value[6]) {
+                    case 0:
+                        //-----right mode-------------------------------------------------------------------------------------------------------------//
+
+                        break;
+                    case 1:
+                        //-----left mode--------------------------------------------------------------------------------------------------------------//
+
+                        if(go_waitmode == 0) {
 
-//-----テスト自動プログラム（練習・動作確認用--------------------------------------------------------------------//
-#ifdef HARUROBO_TEST_MODE
-        
-        if(go_waitmode == 0){
-           
-           while(1);
-           //can_start();
-           //set_cond(2,1,-700,1,-700);
-           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,-1610,2000,9,1000,5,0.1,10,0.1,600,0);
-           gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0);
-           MaxonControl(0,0,0,0);
-           set_cond(2,0,-1243,1,1080);
-           pos_correction(-1610,1250,0,1,0);
-           wait(0.5);
+                            //can_start();
+                            //set_cond(2,1,-700,1,-700);
+                            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,-1610,2000,9,1000,5,0.1,10,0.1,600,0);
+                            gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0);
+                            MaxonControl(0,0,0,0);
+                            set_cond(2,0,-1243,1,1080);
+                            pos_correction(-1610,1250,0,1,0);
+                            wait(0.5);
+
+                            gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0);
+                            purecurve(3,1,1,-1610,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(1,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,1,1);
+                            wait(0.5);
 
-           gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0);
-           purecurve(3,1,1,-1610,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(1,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,1,1);
-           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(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
-           MaxonControl(0,0,0,0);
-           pos_correction(-2700,6647,-90,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,-350,6000,9,1000,5,0.1,10,0.1,600,-180);
-           gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180);
-           set_cond(2,1,1243,1,4000);
-           gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
-           MaxonControl(0,0,0,0);
-           pos_correction(-400,4500,-180,1,1);
+                            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(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
+                            MaxonControl(0,0,0,0);
+                            pos_correction(-2700,6647,-90,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,-350,6000,9,1000,5,0.1,10,0.1,600,-180);
+                            gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180);
+                            set_cond(2,1,1243,1,4000);
+                            gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
+                            MaxonControl(0,0,0,0);
+                            pos_correction(-400,4500,-180,1,1);
+
+                            go_waitmode = 1;
 
-           go_waitmode = 1;
-        
-        }else if(go_waitmode == 1){
-            
-        MaxonControl(0,0,0,0);
-        
+                        } else if(go_waitmode == 1) {
+
+                            MaxonControl(0,0,0,0);
+
+                        }
+                        break;
+                }
+//-----wait mode----------------------------------------------------------------------------------------------------------------------//
+            case 0:
+
+                calc_xy(0,1,1);
+                ashi_led();
+                MaxonControl(0,0,0,0);
+
+                break;
+//-----manual mode--------------------------------------------------------------------------------------------------------------------//
+            case 2:
+
+                ManualOut(250,100,500,200);
+
+                break;
         }
-        
-        
-#endif 
-//--------------------------------------------------------------------------------------------------------//
-
-
-
-
-//-----本番用自動プログラム（右側フィールド用）-------------------------------------------------------------------//
-#ifdef HARUROBO_RIGHT_MODE
-
-        //ここに本番用自動プログラム（右側フィールド用）を書く
-
-#endif 
-//---------------------------------------------------------------------------------------------------------//
-
-
-
-//-----本番用自動プログラム（左側フィールド用）-------------------------------------------------------------------//
-#ifdef HARUROBO_LEFT_MODE //
-
-        //ここに本番用自動プログラム（左側フィールド用）を書く
-
-#endif 
-//---------------------------------------------------------------------------------------------------------//
-        MaxonControl(0,0,0,0);
-        break;
-            
-//-----wait mode----------------------------------------------------------------------------------------------------------------------//    
-        case 0:
-        
-        calc_xy(0,1,1);
-        ashi_led();
-        MaxonControl(0,0,0,0);
-        printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y);
-        
-        break;
-            
-//-----manual mode--------------------------------------------------------------------------------------------------------------------//
-        case 2:
-
-        ManualOut(250,100,500,200);
-                        
-        break;
 //------------------------------------------------------------------------------------------------------------------------------------//
     }
-}
 }
\ No newline at end of file

--- a/manual/manual.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/manual/manual.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -11,7 +11,7 @@
 
 #define PI 3.141592
 
-int id1_value[6]={0};
+int id1_value[7]= {0};
 
 //-----手動用の変数宣言--------------------------------------------------------------------------//
 int stick_theta;  //ジョイスティックの角度
@@ -20,48 +20,49 @@
 int manual_rout;  //旋回速度
 
 void CalManualOut(int v,int r_out)  //vはθ方向の速度、r_outは旋回速度(正の値)
-                                 //PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる
+//PS3ジョイスティックの x=127.5 かつ y>127.5 の直線を0°としてθをとる
 {
     stick_theta = (short)((id1_value[1]<<8) | id1_value[2]);
     //debug_printf("stick = %d\n\r",stick_theta);
-    
+
     //ジョイスティック方向の速度をフィールド座標系の速度に変換
-    manual_xout = v * sin(PI*stick_theta/180);  
+    manual_xout = v * sin(PI*stick_theta/180);
     manual_yout = -v * cos(PI*stick_theta/180);
-    
+
     //フィールド座標系の速度を機体座標系の速度に変換
     manual_realxout = manual_xout * cos(PI*now_angle/180) + manual_yout * sin(PI*now_angle/180);
     manual_realyout = -manual_xout * sin(PI*now_angle/180) + manual_yout * cos(PI*now_angle/180);
-    
-    if(id1_value[4] == 1){  //旋回速度を代入
+
+    if(id1_value[4] == 1) { //旋回速度を代入
         manual_rout = 0;
-    }else if(id1_value[4] == 2){
+    } else if(id1_value[4] == 2) {
         manual_rout = r_out;
-    }else if(id1_value[4] == 3){
+    } else if(id1_value[4] == 3) {
         manual_rout = -r_out;
     }
-    
-    
+
+
 }
 
-void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r){
-    
+void ManualOut(int slow_v, int slow_r, int fast_v, int fast_r)
+{
+
     calc_gyro();
 
-    if(id1_value[3]==1){  //BOTTONR1押したら減速
+    if(id1_value[3]==1) { //BOTTONR1押したら減速
         CalManualOut(slow_v,slow_r);
-    }else{
+    } else {
         CalManualOut(fast_v,fast_r);
     }
-                
-    if(id1_value[5] == 1){  //ニュートラルポジションなら出力0
+
+    if(id1_value[5] == 1) { //ニュートラルポジションなら出力0
         output(0,0,0,0);
         base(manual_rout,manual_rout,manual_rout,manual_rout,4095);
-    }else{
+    } else {
         CalMotorOut(manual_realxout,manual_realyout,0);
         base(GetMotorOut(0)+manual_rout,GetMotorOut(1)+manual_rout,GetMotorOut(2)+manual_rout,GetMotorOut(3)+manual_rout,4095);
     }
-                
+
     MaxonControl(m1,m2,m3,m4);
     debug_printf("m1=%d m2=%d m3=%d m4=%d now_angle=%f\n\r",m1,m2,m3,m4,now_angle);
 }
\ No newline at end of file

--- a/manual/manual.h	Sat Mar 02 07:18:38 2019 +0000
+++ b/manual/manual.h	Sat Mar 02 07:48:18 2019 +0000
@@ -1,7 +1,7 @@
 #ifndef HARUROBO2019_MANUAL
 #define HARUROBO2019_MANUAL
 
-extern int id1_value[6];
+extern int id1_value[7];
 
 void CalManualOut(int v,int r_out);
 

--- a/movement/movement.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/movement/movement.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -11,10 +11,12 @@
 
 #define PI 3.141592
 
-char ashi_data[4]={0};
+char can_ashileddata[1]= {0};
+int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3;
 
+Ec EC1(p22,p21,NC,500,0.05);
 Ec EC2(p26,p8,NC,500,0.05);
-Ec EC1(p22,p21,NC,500,0.05);
+
 Ticker ec_ticker;  //ec角速度計算用ticker
 
 R1370P gyro(p28,p27);
@@ -40,25 +42,26 @@
  *info.nowY.usw → 超音波センサーにより求めた機体位置のy座標
 */
 
-typedef struct{ //使用センサーの種類
+typedef struct { //使用センサーの種類
     double usw;  //超音波センサー
     double enc;  //エンコーダ
     double gyro; //ジャイロ
     //double line;//ラインセンサー
-}robo_sensor;
+} robo_sensor;
 
-typedef struct{ //機体情報の種類
+typedef struct { //機体情報の種類
     robo_sensor angle; //←機体角度は超音波センサーやラインセンサーからも算出可能なので一応格納先を用意したが、ジャイロの値を完全に信用してもいいかも
     robo_sensor nowX;
     robo_sensor nowY;
-}robo_data;
+} robo_data;
 
-robo_data info={{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
+robo_data info= {{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-void UserLoopSetting_sensor(){
-    
+void UserLoopSetting_sensor()
+{
+
     gyro.initialize();
     ec_ticker.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
     EC1.setDiameter_mm(25.5);
@@ -91,40 +94,43 @@
         else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL));
         else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR));
         else                                                               output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR));
-      } else {
+    } else {
         output(FL,BL,BR,FR);
     }
 }
 
-void ashi_led(){
-    
-    if(now_angle > -1 && now_angle < 1){
-        ashi_data[0] = 1;
-    }else{
-        ashi_data[0] = 0;
+void ashi_led()
+{
+
+    if(now_angle > -1 && now_angle < 1) {
+        can_ashileddata0_0 = 1;
+    } else {
+        can_ashileddata0_0 = 0;
     }
-    
-    if(now_angle > 350){
-        ashi_data[1] = 1;
-    }else{
-        ashi_data[1] = 0;
+
+    if(now_angle > 350) {
+        can_ashileddata0_1 = 1;
+    } else {
+        can_ashileddata0_1 = 0;
     }
-    
-    if(now_x > -1 && now_x < 1){
-        
-        ashi_data[2] = 1;
-    }else{
-        ashi_data[2] = 0;
+
+    if(now_x > -1 && now_x < 1) {
+        can_ashileddata0_2 = 1;
+    } else {
+        can_ashileddata0_2 = 0;
     }
-    
-    if(now_y > -1 && now_y < 1){
-        ashi_data[3] = 1;
-    }else{
-        ashi_data[3] = 0;
+
+    if(now_y > -1 && now_y < 1) {
+        can_ashileddata0_3 = 1;
+    } else {
+        can_ashileddata0_3 = 0;
     }
+
+    can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4);
 }
 
-void calc_gyro(){
+void calc_gyro()
+{
     now_angle=gyro.getAngle();  //ジャイロの値読み込み
 }
 
@@ -145,109 +151,112 @@
     info.nowY.enc = info.nowY.enc - d_y;  //微小時間毎に座標に加算
 }
 
-void set_cond(int t, int px, double bx, int py, double by){  //超音波センサーを使用するときの条件設定関数
+void set_cond(int t, int px, double bx, int py, double by)   //超音波センサーを使用するときの条件設定関数
+{
 //引数の詳細は関数"calc_xy_usw"参照
-  
-   xy_type = t;
-   
-   pm_typeX = px;
-   x_base = bx;
-   
-   pm_typeY = py;
-   y_base = by;   
+
+    xy_type = t;
+
+    pm_typeX = px;
+    x_base = bx;
+
+    pm_typeY = py;
+    y_base = by;
 }
 
-void calc_xy_usw(double tgt_angle){  //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
+void calc_xy_usw(double tgt_angle)   //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
+{
 //tgt_angle:機体の目標角度（運動初期角度と同じ/今大会では0,90,180のみ）
 //xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む）
 //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=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
-    
+
     now_angle=gyro.getAngle();  //ジャイロの値読み込み
-    
-    if(tgt_angle==0){
-      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));
-           
-      }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));
-          
-      }
-      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));
-          
-      }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));
-          
-      }
-    
-    }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));
-           
-      }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));
-          
-      }
-      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));
-          
-      }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));
-          
-      }
-        
-    }else if(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));
-           
-      }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));
-          
-      }
-      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));
-          
-      }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));
-          
-      }
+
+    if(tgt_angle==0) {
+        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));
+
+        } 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));
+
+        }
+        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));
+
+        } 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));
+
+        }
+
+    } 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));
+
+        } 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));
+
+        }
+        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));
+
+        } 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));
+
+        }
+
+    } else if(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));
+
+        } 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));
+
+        }
+        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));
+
+        } 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));
+
+        }
     }
 }
 
-void calc_xy(double target_angle, double u,double v){
+void calc_xy(double target_angle, double u,double v)
+{
 //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する
-    
-   calc_xy_enc();
-   
-   if(u != 1 || v != 1){
-      calc_xy_usw(target_angle);  //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
-   }
-    
+
+    calc_xy_enc();
+
+    if(u != 1 || v != 1) {
+        calc_xy_usw(target_angle);  //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
+    }
+
     now_x = u * info.nowX.enc + (1-u) * info.nowX.usw;
     now_y = v * info.nowY.enc + (1-v) * info.nowY.usw;
-    
+
     /*if(now_x >-1 && now_x <1 && now_y >-1 && now_y <1){  //スタート時の0合わせ用
         ec_led = 1;
     }else{
         ec_led = 0;
     }
-    
+
     if(now_angle >-0.5 && now_angle <0.5){
         gyro_led = 1;
     }else{
@@ -391,12 +400,12 @@
 
         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);  //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);
-        
+
         if(t == (90/theta))break;
         if(id1_value[0] != 1)break;
     }
@@ -417,7 +426,7 @@
     r_setPDparam(r_p,r_d);  //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
     set_r_out(r_out_max);  //旋回時の最大出力値設定関数
     set_target_angle(target_angle);  //機体目標角度設定関数
-    
+
     while (1) {
 
         calc_xy(target_angle,u,v);
@@ -430,50 +439,51 @@
 
         base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);
         //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);
 
         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; 
+        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:補正終了とみなす目標位置からの機体の位置のズレ
     double out;
-   
+
     calc_xy(tgt_angle, u, v);
- 
-    while(1){  //機体の位置を目標領域（目標座標＋許容誤差）に収める
+
+    while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
         gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,50,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){
-    
+
+    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) {
+        } 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;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
-        
+
     }
     MaxonControl(0,0,0,0);
 }
\ No newline at end of file

--- a/movement/movement.h	Sat Mar 02 07:18:38 2019 +0000
+++ b/movement/movement.h	Sat Mar 02 07:48:18 2019 +0000
@@ -1,7 +1,7 @@
 #ifndef HARUROBO2019_MOVEMENT
 #define HARUROBO2019_MOVEMENT
 
-extern char ashi_data[4];
+extern char can_ashileddata[1];
 
 extern int16_t m1,m2,m3,m4;
 
@@ -34,7 +34,7 @@
                double r_p,double r_d,
                double r_out_max,
                double target_angle);
-                
+
 void gogo_straight(double u, double v, double x1_point,double y1_point,  //直線運動プログラム
                    double x2_point,double y2_point,
                    double speed1,double speed2,

--- a/pathfollowing/PathFollowing.cpp	Sat Mar 02 07:18:38 2019 +0000
+++ b/pathfollowing/PathFollowing.cpp	Sat Mar 02 07:48:18 2019 +0000
@@ -28,8 +28,7 @@
     double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
     double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
 
-
-    //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解*/
+    //double VectorOut_P[2]= {0}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解
 
 ///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
 
@@ -91,13 +90,13 @@
         if(diff_tgt > diff_st_tgt) {
             diff_tgt = diff_st_tgt;
         }
-        
+
         p_param=(diff_tgt/diff_st_tgt);
 
         double speed3 = speed2 + (speed1-speed2)*p_param;
 
         double VectorOut_P[2] = {speed3*UnitVector_P[0], speed3*UnitVector_P[1]};
-         
+
         *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180);
         *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180);
         *ad_r_out = out_dutyR;
@@ -107,13 +106,13 @@
         if(diff_st > diff_st_tgt) {
             diff_st = diff_st_tgt;
         }
-             
+
         p_param=(diff_st/diff_st_tgt);
 
         double speed4 = speed1 + (speed2-speed1)*p_param;
 
         double VectorOut_P[2] = {speed4*UnitVector_P[0], speed4*UnitVector_P[1]};
-         
+
         *ad_x_out = (VectorOut_P[0]+VectorOut_Q[0])*cos(-now_angle*3.141592/180)-(VectorOut_P[1]+VectorOut_Q[1])*sin(-now_angle*3.141592/180);
         *ad_y_out = (VectorOut_P[0]+VectorOut_Q[0])*sin(-now_angle*3.141592/180)+(VectorOut_P[1]+VectorOut_Q[1])*cos(-now_angle*3.141592/180);
         *ad_r_out = out_dutyR;

--- a/pathfollowing/PathFollowing.h	Sat Mar 02 07:18:38 2019 +0000
+++ b/pathfollowing/PathFollowing.h	Sat Mar 02 07:48:18 2019 +0000
@@ -19,7 +19,7 @@
 //ベクトルABに平行方向の出力値設定関数
 
 void q_setPDparam(double q_p,double q_d);
-//ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 
+//ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
 
 void r_setPDparam(double r_p,double r_d);
 //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数