春ロボ1班(元F3RC4班+)
3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 12:762061580168
- Parent:
- 11:9db93bce4eef
- Child:
- 13:f9867c3b97b0
--- a/main.cpp Tue Mar 19 02:30:34 2019 +0000
+++ b/main.cpp Tue Mar 19 14:31:24 2019 +0000
@@ -59,19 +59,21 @@
flag = 0;
UserLoopSetting_enc_right();
if(T1 == 0) { //スタート位置からみかんの木まで移動
- /*
- gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0);
- purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0);
- purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0);
+
+ 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(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
set_cond(2,1,1050,1,1076);
- gogo_straight(0,0,1672,2000,1672,1600,1000,200,5,0.1,10,0.1,600,0);
+ 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(1672,1600,0,0,0,25);
enc_correction(1,1);
- gogo_straight(0,0,1672,1600,1672,1400,200,200,5,0.1,10,0.1,600,0);
- */
- printf("t1 = 0\n\r");
- wait(1);
+ 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);
+
+ //printf("t1 = 0\n\r");
+ //wait(1);
T1++;
}
if(T1 == 1) {
@@ -85,21 +87,21 @@
}
}
if(T1 == 2) { //みかんの木から三宝置き場まで移動
- /*gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0);
- purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0);
- purecurve(1,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,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,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(2962,4500,90,1,1);
set_cond(2,1,2462,0,6000);
- gogo_straight(0,0,2962,4500,2850,4500,200,200,5,0.1,10,0.1,800,90);
+ 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);
- pos_correction(2850,4500,90,0,0,10);
enc_correction(1,1);
- wait(0.5);*/
- printf("t1 = 2\n\r");
- wait(1);
+
+ //printf("t1 = 2\n\r");
+ //wait(1);
T1++;
}
if(T1 == 3) {
@@ -113,19 +115,22 @@
}
}
if(T1 == 4) { //三宝置き場からりんごの木まで移動
- /*gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90);
- purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90);
- purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90);
- purecurve(2,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,0,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,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90);
+ purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90);
+ purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90);
+ purecurve(2,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);
- pos_correction(2700,6600,90,0,0,15);
+ set_cond(1,0,0,1,6038);
+ pos_correction(2500,6610,90,1,0,8);
+ set_cond(2,0,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);
- wait(0.5);*/
- printf("t1 = 4\n\r");
- wait(1);
+
+ // printf("t1 = 4\n\r");
+ // wait(1);
T1++;
}
if(T1 == 5) {
@@ -139,20 +144,27 @@
}
}
if(T1 == 6) { //りんごの木からお供え台まで移動
- /*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);
+
+ 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,1,19,0,6962); //要修正
+ purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正
+ enc_correction(1,0);
+ set_cond(0,1,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,0,1000,1,4000);
+ 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(-1672,1600,0,0,0,25);
+ pos_correction(620,4370,180,0,0,30);
enc_correction(1,1);
- 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);
- */
- printf("t1 = 6\n\r");
- wait(1);
+
+ // printf("t1 = 6\n\r");
+ // wait(1);
T1++;
}
if(T1 == 7) {
@@ -201,11 +213,11 @@
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,4820,max_speed,end_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);
set_cond(2,0,-2462,0,6000);
- pos_correction(-2962,4837.5,-90,0,0,7);
- gogo_straight(0,0,-2962,4837.5,-2800,4837.5,300,200,5,0.1,10,0.1,800,-90);
+ 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);
@@ -226,12 +238,12 @@
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,6593,turn_speed,end_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,6050);
- pos_correction(-2500,6600,-90,1,0,8);
+ 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,6600,-2700,6600,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
+ 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);
@@ -249,20 +261,21 @@
}
if(T1 == 6) { //りんごの木からお供え台まで移動
set_cond(1,0,0,0,6962);
- gogo_straight(1,0,-2700,6600,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
+ 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,0,0,-850,6550,-519,6000,9,turn_speed,10,0.1,10,0.1,600,-180);
+ purecurve(8,0,0,-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,-519,6000,-519,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
- gogo_straight(0,1,-519,5500,-519,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
+ 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,5000,-519,4600,max_speed,end_speed,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,4370,-180,0,0,30);
+ pos_correction(-620,4370,-180,0,0,30);
enc_correction(1,1);
MaxonControl(0,0,0,0);
@@ -273,32 +286,6 @@
printf("ashi finished\n\r");
MaxonControl(0,0,0,0);
- /*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);
- MaxonControl(0,0,0,0);
- set_cond(2,0,0,0,7000);
- //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);
- 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,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(-3112,3500,0,0,0,15);
- enc_correction(1,1);
- MaxonControl(0,0,0,0);*/
-
if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
if(id1_value[6] != flag)break;
}