春ロボ1班(元F3RC4班+)
3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 9:7667dcfc7ce5
- Parent:
- 8:2ba338b4590e
- Child:
- 10:c741191360de
--- a/main.cpp Thu Mar 07 02:54:55 2019 +0000
+++ b/main.cpp Wed Mar 13 03:24:49 2019 +0000
@@ -16,8 +16,8 @@
int go_waitmode = 0;
//-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
-//DigitalOut cansend_led(LED1); //canread -> on //can.cpp
-//DigitalOut canread_led(LED2); //cansend -> on //can.cpp
+//DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
+//DigitalOut canread_led(LED2); //canread -> on //can.cpp
//DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
//////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
@@ -45,58 +45,99 @@
switch(id1_value[6]) {
case 0:
//-----right mode-------------------------------------------------------------------------------------------------------------//
- if(T1 == 0) {
-
- //スタート位置からみかんの木まで移動
+ flag = 0;
+ if(T1 == 0) { //スタート位置からみかんの木まで移動
+ 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);
T1++;
-
}
if(T1 == 1) {
while(1) {
- wait(0.5);
+ wait(0.1);
if(T1 == 2) {
break;
}
}
}
- if(T1 == 2) {
-
- //みかんの木から三宝置き場まで移動
+ if(T1 == 2) { //みかんの木から三宝置き場まで移動
+ 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);
+ 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);
+ enc_correction(1,1);
+ wait(0.5);
T1++;
}
if(T1 == 3) {
while(1) {
- wait(0.5);
+ wait(0.1);
if(T1 == 4) {
break;
}
}
}
- if(T1 == 4) {
-
- //三宝置き場からりんごの木まで移動
+ if(T1 == 4) { //三宝置き場からりんごの木まで移動
+ 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);
+ MaxonControl(0,0,0,0);
+ pos_correction(-2700,6600,-90,0,0);
+ enc_correction(1,1);
wait(0.5);
T1++;
}
if(T1 == 5) {
while(1) {
-
- wait(0.5);
+ wait(0.1);
if(T1 == 6) {
break;
}
}
}
- if(T1 == 6) {
-
- //りんごの木からお供え台まで移動
- wait(0.5);
+ if(T1 == 6) { //りんごの木からお供え台まで移動
+ 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(2,1,-1000,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);
+ enc_correction(1,1);
+ MaxonControl(0,0,0,0);
T1++;
}
+ if(T1 == 7) {
+ 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;
case 1:
//-----left mode--------------------------------------------------------------------------------------------------------------//
+ flag = 1;
break;
}