春ロボ1班(元F3RC4班+)
3/20 13:39
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 14:6c1b36c55915
- Parent:
- 13:f9867c3b97b0
--- a/main.cpp Tue Mar 19 17:03:27 2019 +0000
+++ b/main.cpp Wed Mar 20 04:39:06 2019 +0000
@@ -54,13 +54,16 @@
switch(id1_value[6]) {
case 0:
//-----right mode-------------------------------------------------------------------------------------------------------------//
- //未修正
-
flag = 0;
UserLoopSetting_enc_right();
- if(T1 == 0) { //スタート位置からみかんの木まで移動
+
+ if(T1 == 0) { //スタート位置から少し移動
gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
+ T1++;
+ }
+ if(T1 == 1) { //みかんの木まで移動して掴む
+
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);
@@ -71,22 +74,18 @@
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 = 0\n\r");
- //wait(1);
T1++;
}
- if(T1 == 1) {
+
+ if(T1 == 2) { //みかんを持ち上げる
while(1) {
wait(0.1);
- //calc_xy(0,0,0);
- if(T1 == 2) {
- // enc_correction(1,1);
+ if(T1 == 3) {
break;
}
}
}
- if(T1 == 2) { //みかんの木から三宝置き場まで移動
+ if(T1 == 3) { //みかんの木から三宝置き場まで移動
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);
@@ -100,21 +99,18 @@
MaxonControl(0,0,0,0);
enc_correction(1,1);
- //printf("t1 = 2\n\r");
- //wait(1);
+
T1++;
}
- if(T1 == 3) {
+ if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる
while(1) {
wait(0.1);
- //calc_xy(90,0,0);
- if(T1 == 4) {
- // enc_correction(1,1);
+ if(T1 == 5) {
break;
}
}
}
- if(T1 == 4) { //三宝置き場からりんごの木まで移動
+ if(T1 == 5) { //三宝置き場からりんごの木まで移動
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);
@@ -129,21 +125,17 @@
MaxonControl(0,0,0,0);
enc_correction(1,1);
- // printf("t1 = 4\n\r");
- // wait(1);
T1++;
}
- if(T1 == 5) {
+ if(T1 == 6) { //りんごを持ち上げる
while(1) {
wait(0.1);
- //calc_xy(90,0,0);
- if(T1 == 6) {
- // enc_correction(1,1);
+ if(T1 == 7) {
break;
}
}
}
- if(T1 == 6) { //りんごの木からお供え台まで移動
+ if(T1 == 7) { //りんごの木からお供え台まで移動
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);
@@ -163,11 +155,9 @@
enc_correction(1,1);
MaxonControl(0,0,0,0);
- // printf("t1 = 6\n\r");
- // wait(1);
T1++;
}
- if(T1 == 7) {
+ if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く
while(1) {
printf("ashi finished\n\r");
MaxonControl(0,0,0,0);
@@ -176,15 +166,19 @@
}
}
break;
-
case 1:
//-----left mode--------------------------------------------------------------------------------------------------------------//
flag = 1;
- printf("start\n\r");
UserLoopSetting_enc_left();
- if(T1 == 0) { //スタート位置からみかんの木まで移動
+
+ if(T1 == 0) { //スタート位置から少し移動
gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
+
+ T1++;
+ }
+ if(T1 == 1) { //みかんの木まで移動して掴む
+
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);
@@ -198,17 +192,17 @@
T1++;
}
- if(T1 == 1) {
+
+ if(T1 == 2) { //みかんを持ち上げる
while(1) {
wait(0.1);
- calc_xy(0,0,0);
- if(T1 == 2) {
- enc_correction(1,1);
+ if(T1 == 3) {
break;
}
}
}
- if(T1 == 2) { //みかんの木から三宝置き場まで移動
+ if(T1 == 3) { //みかんの木から三宝置き場まで移動
+
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);
@@ -221,19 +215,19 @@
MaxonControl(0,0,0,0);
enc_correction(1,1);
+
T1++;
}
- if(T1 == 3) {
+ if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる
while(1) {
wait(0.1);
- calc_xy(-90,0,0);
- if(T1 == 4) {
- enc_correction(1,1);
+ if(T1 == 5) {
break;
}
}
}
- if(T1 == 4) { //三宝置き場からりんごの木まで移動
+ if(T1 == 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);
@@ -249,17 +243,16 @@
T1++;
}
- if(T1 == 5) {
+ if(T1 == 6) { //りんごを持ち上げる
while(1) {
wait(0.1);
- calc_xy(-90,0,0);
- if(T1 == 6) {
- enc_correction(1,1);
+ if(T1 == 7) {
break;
}
}
}
- if(T1 == 6) { //りんごの木からお供え台まで移動
+ if(T1 == 7) { //りんごの木からお供え台まで移動
+
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);
@@ -281,18 +274,14 @@
T1++;
}
- if(T1 == 7) {
+ if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く
while(1) {
printf("ashi finished\n\r");
MaxonControl(0,0,0,0);
-
if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
if(id1_value[6] != flag)break;
}
}
-
-
-
break;
}
//-----wait mode----------------------------------------------------------------------------------------------------------------------//
@@ -455,7 +444,7 @@
T1++;
}
-
+
if(T1 == 1) {
while(1) {
ManualOut(250,100,500,200);
@@ -547,7 +536,7 @@
break;
}
- // ManualOut(250,100,500,200);
+ // ManualOut(250,100,500,200);
//go_waitmode = 0;
break;