改良版位置補正プログラム動作未確認

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

Files at this revision

API Documentation at this revision

Comitter:
yuki0701
Date:
Fri Mar 01 00:48:07 2019 +0000
Parent:
2:7cba05e70367
Child:
4:317c53a674fa
Commit message:
a;

Changed in this revision

can/can.cpp 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
maxonsetting/maxonsetting.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
--- a/can/can.cpp	Tue Feb 26 05:10:27 2019 +0000
+++ b/can/can.cpp	Fri Mar 01 00:48:07 2019 +0000
@@ -14,12 +14,12 @@
 void can_readsend()
 {
     CANMessage msg;
-    
+
     if(can1.read(msg)) {
         canread_led = 1;
-        
-        if(msg.id == 1){  //from main
-        
+
+        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[2] = msg.data[2];
@@ -29,54 +29,56 @@
 
             //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]);
         }
-        
-        if(msg.id == 3){
+
+        /*if(msg.id == 3) {
             usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
             //debug_printf("usw_data1 = %f\n\r",usw_data1);
-        }
-        
-        /*if(msg.id == 4){
-            usw_data2 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
-            //debug_printf("usw_data2 = %f\n\r",usw_data2);
         }*/
-        
-        /*if(msg.id == 5){
-            usw_data3 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
-            //debug_printf("usw_data3 = %f\n\r",usw_data3);
-        }*/
-        
-        /*if(msg.id == 6){
+
+        /*   if(msg.id == 4){
+               usw_data2 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
+               //debug_printf("usw_data2 = %f\n\r",usw_data2);
+           }*/
+
+        /*    if(msg.id == 5){
+                usw_data3 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
+                //debug_printf("usw_data3 = %f\n\r",usw_data3);
+            }*/
+
+        if(msg.id == 6) {
             usw_data4 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
             //debug_printf("usw_data4 = %f\n\r",usw_data4);
-        }*/
-          
-    }else{
+        }
+
+    } else {
         canread_led = 0;
     }
-    
+
     if(can1.write(CANMessage(7,ashi_data,4))) {  //IDを7にして送信
         cansend_led = 1;
-    }else{
+    } else {
         cansend_led = 0;
     }
+
+}
+void can_start()
+{
+
+    while(1) {
+
+        CANMessage msg;
+
+        debug_printf("wait\n\r");
+        wait(0.1);
+        if(can1.read(msg)) {
+            break;
+        }
+    }
 }
 
-void can_start(){
+void UserLoopSetting_can()
+{
 
-    while(1){
-        
-        CANMessage msg;
-        
-        debug_printf("wait\n\r");
-        wait(0.1);
-        if(can1.read(msg)){
-            break;
-        } 
-    } 
-}
-
-void UserLoopSetting_can(){
-    
     can1.frequency(1000000);
     can_ticker.attach(&can_readsend,0.01);  //遅かったら早める
 }
\ No newline at end of file
--- a/main.cpp	Tue Feb 26 05:10:27 2019 +0000
+++ b/main.cpp	Fri Mar 01 00:48:07 2019 +0000
@@ -62,40 +62,40 @@
            gogo_straight(1,1,-1672,2000,-1672,1500,1000,200,5,0.1,10,0.1,600,0);
            MaxonControl(0,0,0,0);
            set_cond(2,0,-1243,1,1076);
-           pos_correction(-1672,1500,0,0,0);
+           pos_correction(-1672,1500,0,0,1);
            wait(0.5);
 
            gogo_straight(1,1,-1672,1500,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
            purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
-           purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,90);
-           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);
+           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);
+           pos_correction(-2962,4500,-90,1,1);
       
            set_cond(0,0,-2462,0,0);
-           gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,90);
+           gogo_straight(0,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,0,1);
+           pos_correction(-2850,4500,-90,0,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);
+           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(1,0,0,0,7000);
-           gogo_straight(1,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,90);
+           gogo_straight(1,0,-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,0);
+           pos_correction(-2700,6647,-90,1,0);
            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,-1243,6647,1000,1000,5,0.1,10,0.1,800,90);
-           purecurve(8,1,1,-1243,6647,-519,6000,9,1000,5,0.1,10,0.1,600,180);
-           gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,180);
+           gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
+           gogo_straight(1,1,-2500,6647,-1243,6647,1000,1000,5,0.1,10,0.1,800,-90);
+           purecurve(8,1,1,-1243,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
+           gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
            set_cond(2,0,0,1,4000);
-           gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,180);
+           gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
            MaxonControl(0,0,0,0);
            pos_correction(-519,4500,180,0,0);
 
--- a/maxonsetting/maxonsetting.cpp	Tue Feb 26 05:10:27 2019 +0000
+++ b/maxonsetting/maxonsetting.cpp	Fri Mar 01 00:48:07 2019 +0000
@@ -23,10 +23,10 @@
 //-----mbed----------------------------------------------------------------------------//
 SPI spi(p5,p6,p7);
 
-DigitalOut ss_md1(p15);           //エスコンの設定
-DigitalOut ss_md2(p16);
-DigitalOut ss_md3(p17);
-DigitalOut ss_md4(p18);
+DigitalOut ss_md1(p17);           //エスコンの設定
+DigitalOut ss_md2(p18);
+DigitalOut ss_md3(p15);
+DigitalOut ss_md4(p16);
 
 DigitalOut md_enable(p25);
 //DigitalIn md_ch_enable(p10);        // check enable switch is open or close
--- a/movement/movement.cpp	Tue Feb 26 05:10:27 2019 +0000
+++ b/movement/movement.cpp	Fri Mar 01 00:48:07 2019 +0000
@@ -13,8 +13,8 @@
 
 char ashi_data[4]={0};
 
-Ec EC1(p8,p26,NC,500,0.05);
-Ec EC2(p21,p22,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);
@@ -235,12 +235,9 @@
     
    calc_xy_enc();
    
-   if(u != 1){
+   if(u != 1 || v != 1){
       calc_xy_usw(target_angle);  //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
    }
-   if(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;
@@ -450,7 +447,7 @@
     calc_xy(tgt_angle, u, v);
  
     while(1){  //機体の位置を目標領域(目標座標+許容誤差)に収める
-        gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,100,0,5,0.1,10,0.1,500,tgt_angle);
+        gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,100,5,0.1,10,0.1,500,tgt_angle);
         MaxonControl(0,0,0,0);
     
         calc_xy(tgt_angle, u, v);
@@ -475,7 +472,7 @@
             MaxonControl(out,out,out,out);
         }
  
-        if(tgt_angle - 2 < now_angle && now_angle < tgt_angle + 2) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+        if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
         
     }
     MaxonControl(0,0,0,0);