Diff: main.cpp

Revision:

11:8eda88c96bb0

Parent:

10:cf5c159e729a

--- a/main.cpp	Fri Mar 08 07:09:33 2019 +0000
+++ b/main.cpp	Sat Dec 26 09:00:06 2020 +0000
@@ -17,6 +17,17 @@
 
 int go_waitmode = 0;
 
+/*------------運動時の各速度の設定----------*/
+double st_speed = 500; // 移動開始時の速度
+double max_speed = 2000; // 直線移動時の最大速度
+double mid_speed = 600; // 直線移動時の中間速度
+double turn_speed = 1000; // 旋回時の速度
+double end_speed = 200; // 停止直前の速度
+double fruit_speed = 200; // 果物を取る時の速度
+/*--------------------------------------*/
+
+
+
 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
 //DigitalOut cansend_led(LED1);  //canread -> on                //can.cpp
 //DigitalOut canread_led(LED2);  //cansend -> on                //can.cpp
@@ -42,83 +53,103 @@
 
     id1_value[0] = 1;
     
-    //while(1);
 
-    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,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
-    set_cond(2,0,-1050,1,1076);
-    gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
-    MaxonControl(0,0,0,0);
-    pos_correction(-1672,1400,0,0,0);
-    enc_correction(1,1);
-    wait(0.5);
+    while(1){
+         //int m1=0,m2=0,m3=0,m4=0;
+         int m1=4095,m2=4095,m3=4095,m4=4095;
+         MaxonControl(m1,m2,m3,m4); 
+         debug_printf("m1=%d m2=%d m3=%d m4=%d\n\r",m1,m2,m3,m4);
+        }
 
-    gogo_straight(1,1,-1672,1400,-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); //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);
+    gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
+    gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
+    purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+    purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
+    set_cond(2,0,-1050,1,1076);
+    gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
     MaxonControl(0,0,0,0);
-    pos_correction(-2962,4500,-90,1,1);
-    set_cond(2,0,-2462,0,6000);
-    gogo_straight(0,0,-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,0);
+    pos_correction(-1672,1600,0,0,0,25);
     enc_correction(1,1);
-    wait(0.5);
+    gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
+    MaxonControl(0,0,0,0);
+    
+    //wait(3);
 
-    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,6550,9,1000,5,0.1,10,0.1,800,-90);
-    gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90);
-    set_cond(2,1,-3500,1,6050);
-    gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90);
+    gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
+    purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+    purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,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,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
+    gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90);
     MaxonControl(0,0,0,0);
-    pos_correction(-2700,6600,-90,0,0);
+    set_cond(2,0,-2462,0,6000);
+    pos_correction(-2962,4837.5,-90,0,0,10);
+    gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90);
+    MaxonControl(0,0,0,0);
     enc_correction(1,1);
-    wait(0.5);
+
+    gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
+    purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
+    purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
+    purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
+    gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
+    MaxonControl(0,0,0,0);
+    set_cond(1,0,0,1,6038);
+    pos_correction(-2500,6610,-90,1,0,8);
+    set_cond(2,1,-3162,1,6038);
+    gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
+    MaxonControl(0,0,0,0);
+    enc_correction(1,1);
 
-    gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90);
-    gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90);
-    gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90);
-    purecurve(8,1,1,-1000,6550,-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(1,0,0,0,6962);
+    gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
+    gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);
+    
+    gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
+    enc_correction(0,1);
+     //set_cond(2,0,-19,0,6962);
+    purecurve(8,1,1,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正
+    //enc_correction(1,0);
+    set_cond(0,0,-19,0,0);
+    gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
+    gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
     set_cond(2,1,-1000,1,4000);
-    gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
+    gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
     MaxonControl(0,0,0,0);
-    pos_correction(-519,4500,-180,0,0);
+    pos_correction(-620,4370,-180,0,0,30);
     enc_correction(1,1);
     MaxonControl(0,0,0,0);
 
+
+
 //以下自動で初期位置に戻るプログラム
     wait(3);
-    
-    gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180);
-    gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180);
-    gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180);
+
+    gogo_straight(1,1,-620,4500,-620,5000,200,1000,5,0.1,10,0.1,600,-180);
+    gogo_straight(1,1,-620,5000,-620,6000,1000,1000,5,0.1,10,0.1,600,-180);
+    gogo_straight(1,1,-620,6000,-620,6600,1000,200,5,0.1,10,0.1,600,-180);
     MaxonControl(0,0,0,0);
     set_cond(2,0,0,0,7000);
-    pos_correction(-519,6600,-180,0,0);
+    //pos_correction(-519,6600,-180,0,0,30);
+    calc_xy(-180,0,0);
     enc_correction(1,1);
-    gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180);
+    gogo_straight(1,1,-620,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180);
     purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180);
     purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180);
     gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180);
     MaxonControl(0,0,0,0);
     set_cond(2,1,-3500,0,6000);
-    pos_correction(-2962,5500,-180,0,0);
+    //pos_correction(-2962,5500,-180,0,0,30);
     enc_correction(1,1);
+    calc_xy(-180,0,0);
     gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90);
     gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0);
     gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0);
     MaxonControl(0,0,0,0);
     set_cond(2,1,-3500,1,2000);
-    pos_correction(-3000,3500,0,0,0);
+    pos_correction(-3112,3500,0,0,0,15);
     enc_correction(1,1);
     MaxonControl(0,0,0,0);
-    
+
 #endif
 
 #ifdef HARUROBO_MAIN_MODE