mmotor
/
MarsRover_ExoMars____
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: EPOS2 mbed-rtos mbed
Fork of
MarsRover_ExoMars
by 
mmotor
Diff: main.cpp
- Revision:
- 11:6e5ebe9aa84b
- Parent:
- 10:1c42bbd596dc
- Child:
- 12:9e90d7a5a8c6
--- a/main.cpp Mon Oct 17 05:39:38 2016 +0000
+++ b/main.cpp Mon Oct 17 10:22:25 2016 +0000
@@ -39,7 +39,7 @@
bool node1_homed = false;
bool node2_homed = false;
bool node3_homed = false;
-bool node4_homed = true;
+bool node4_homed = false;
bool node5_homed = false;
bool node6_homed = true;
bool node7_homed = true;
@@ -152,7 +152,11 @@
MyEpos7.Reset();
*/
- while(1){
+ while(1){
+ if(MyEpos1.GetError() != 0 or MyEpos2.GetError() != 0 or MyEpos3.GetError() != 0 or MyEpos4.GetError() != 0 or MyEpos5.GetError() != 0 /*or MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0*/){
+ current_step = 7; //FEHLER
+ }
+
switch (current_step){
case 1://HOMING:
led1 = 1;
@@ -195,7 +199,7 @@
MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
MyEpos2.Homing(); //Solarpanel_Innen_Links
MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
- wait(3);
+ wait(2);
node2_homed = true;
break;
}
@@ -212,13 +216,13 @@
break;
}
if (node1_homed == true and node2_homed == true and node3_homed == true and node4_homed == true and node5_homed == true and node6_homed == true and node7_homed == true){
- current_step = 2;//BEREIT;
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
wait(0.1);
MyEpos5.Power(0);
+ current_step = 2;//BEREIT;
}
@@ -233,15 +237,16 @@
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
MyEpos5.SetDigOut(4,1);
- current_step = 6;
wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
-// MyEpos4.Power(1);
+ MyEpos4.Power(1);
MyEpos5.Power(1);
+ current_step = 6;
+ wait(0.5);
}
- wait_ms(10);
+ wait_ms(10);
led2 = 0;
break;
@@ -257,35 +262,40 @@
switch (usv_step){
case 1:
wait(0.1);
- if (node1_homed == true){
- MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links
+// if (node1_homed == true){
+// MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links
MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
wait(0.1);
- }
- if (node3_homed == true){
- MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
+// }
+// if (node3_homed == true){
+// MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
wait(0.1);
- }
+// }
/* if (node7_homed == true){
MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
wait(0.1);
}
*/ wait(0.1);
- usv_step = 2;
+// usv_step = 2;
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
usv_step = 2;
}
break;
case 2:
- if (node2_homed == true and MyEpos1.TargetReached()==1){
+ if (MyEpos1.TargetReached()==1){
wait(1);
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
- MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
wait_ms(10);
}
+ if (MyEpos3.TargetReached()==1){
+ wait(1);
+ MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
+ MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+ wait_ms(10);
+ }
wait(0.1);
/* if (node4_homed == true and MyEpos3.TargetReached()==1){
wait(1);
@@ -298,7 +308,7 @@
wait_ms(10);
}
*/ wait(1);
- if (MyEpos2.TargetReached()==1 /*and MyEpos4.TargetReached()==1 and MyEpos6.TargetReached()==1*/){
+ if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
usv_step = 3;
}
break;
@@ -368,14 +378,15 @@
if (MyEpos5.TargetReached()==1){
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
- MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Aussen Links
+ MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Innen Links
+ MyEpos4.MoveAbsolute(-310000,500,100,100); //Panel Innen Rechts
ausfahren_step = 3;
wait(1);
}
break;
case 3:
- if (MyEpos2.TargetReached()==1){
+ if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
@@ -399,9 +410,9 @@
ausfahren_step = 1;
switch (einfahren_step){
case 1:
- MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links
+/* MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-1000,500,100,100); //Panel Aussen Rechts
- MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
+*/ MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
einfahren_step = 2;
wait(1);
@@ -412,14 +423,15 @@
wait(1);
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
- MyEpos2.MoveAbsolute(0,500,100,100); //Panel Aussen Links
+ MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
+ MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
einfahren_step = 3;
wait(1);
}
break;
case 3:
- if (MyEpos2.TargetReached()==1){
+ if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
wait(1);
MyEpos1.Power(0);