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

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

Files at this revision

API Documentation at this revision

Comitter:
yuki0701
Date:
Mon Feb 25 06:18:09 2019 +0000
Parent:
0:c61c6e4775ca
Child:
2:7cba05e70367
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
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
--- a/can/can.cpp	Wed Feb 13 03:02:19 2019 +0000
+++ b/can/can.cpp	Mon Feb 25 06:18:09 2019 +0000
@@ -31,32 +31,32 @@
         }
         
         if(msg.id == 3){
-            usw_data1 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]);
+            usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
             //printf("usw_data1 = %d:%d,%lf\n\r",msg.data[0],msg.data[1],usw_data1);
             //debug_printf("%f\n\r",usw_data1);
         }
         
-        if(msg.id == 4){
-            usw_data2 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]);
+       /* if(msg.id == 4){
+            usw_data2 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
             //printf("usw_data2 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data2);
-        }
+        }*/
         
         if(msg.id == 5){
-            usw_data3 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]);
+            usw_data3 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
             //printf("usw_data3 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data3);
             //debug_printf("%f\n\r",usw_data3);
         }
         
-        if(msg.id == 6){
-            usw_data4 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]);
+        /*if(msg.id == 6){
+            usw_data4 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
             //printf("usw_data4 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data4);
-        }
+        }*/
           
     }else{
         canread_led = 0;
     }
     
-    if(can1.write(CANMessage(7,ashi_data,1))) {  //IDを7にして送信
+    if(can1.write(CANMessage(7,ashi_data,4))) {  //IDを7にして送信
         cansend_led = 1;
     }else{
         cansend_led = 0;
--- a/main.cpp	Wed Feb 13 03:02:19 2019 +0000
+++ b/main.cpp	Mon Feb 25 06:18:09 2019 +0000
@@ -42,6 +42,8 @@
 
 while(1){
     
+    id1_value[0] = 0;
+    
     switch(id1_value[0]){
             
 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
@@ -51,20 +53,53 @@
 #ifdef HARUROBO_TEST_MODE
         
         if(go_waitmode == 0){
-            
-        purecurve(1,1,1,
-                  0,0,
-                  1000,1000,
-                  9,
-                  1000,5,0.1,10,0.1,500,0);
-        gogo_straight(1,1,
-                     1000,1000,
-                     1000,2000,
-                     1000,1000,
-                     5,0.1,10,0.1,600,0);
-        MaxonControl(0,0,0,0);
-        
-        go_waitmode = 1;
+           
+           can_start();
+          // set_cond(2,1,-700,1,-700);
+           gogo_straight(1,1,-2962,3500,-2962,3000,200,1000,5,0.1,10,0.1,600,0);
+           purecurve(7,1,1,-2962,3000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
+           purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
+           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);
+           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);
+           MaxonControl(0,0,0,0);
+           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);
+           MaxonControl(0,0,0,0);
+           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);
+           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);
+           MaxonControl(0,0,0,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);
+           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);
+           MaxonControl(0,0,0,0);
+           pos_correction(-519,4500,180,0,0);
+
+           go_waitmode = 1;
         
         }else if(go_waitmode == 1){
             
@@ -105,6 +140,7 @@
         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;
             
--- a/movement/movement.cpp	Wed Feb 13 03:02:19 2019 +0000
+++ b/movement/movement.cpp	Mon Feb 25 06:18:09 2019 +0000
@@ -63,6 +63,8 @@
     ec_ticker.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
     EC1.setDiameter_mm(48);
     EC2.setDiameter_mm(48);  //測定輪半径//後で測定
+    info.nowX.enc = -2962; //初期位置の設定
+    info.nowY.enc = 3500;
 }
 
 void calOmega()  //角速度計算関数
@@ -109,6 +111,7 @@
     }
     
     if(now_x > -1 && now_x < 1){
+        
         ashi_data[2] = 1;
     }else{
         ashi_data[2] = 0;
--- a/movement/movement.h	Wed Feb 13 03:02:19 2019 +0000
+++ b/movement/movement.h	Mon Feb 25 06:18:09 2019 +0000
@@ -43,6 +43,6 @@
                    double r_out_max,
                    double target_angle);
 
-void pos_correction();
+void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v);
 
 #endif
\ No newline at end of file