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:
- 12:9e90d7a5a8c6
- Parent:
- 11:6e5ebe9aa84b
- Child:
- 13:cc43e546f310
--- a/main.cpp Mon Oct 17 10:22:25 2016 +0000
+++ b/main.cpp Tue Oct 18 14:16:09 2016 +0000
@@ -44,6 +44,9 @@
bool node6_homed = true;
bool node7_homed = true;
+bool Power_ein = true;
+bool node5_grundposition = false;
+
int homing = false;
int ready = false;
int ausfahren = false;
@@ -52,7 +55,8 @@
int current_step = 1; //HOMING
int ausfahren_step = 1;
int einfahren_step = 1;
-int usv_step = 1;
+int usv_step = 1;
+int ErrorState;
void interrupt_usv(){
current_step = 5;
@@ -63,10 +67,9 @@
led1 = 1;
pc.baud(9600);
- buffering.rise(&interrupt_usv);
buffering.rise(&interrupt_usv); //Interrupt für USV
- wait(0.5); //Wartezeit bis Epos2 70/10 aufgestartet ist
+ wait(0.5); //Wartezeit bis Epos2 24/5 aufgestartet ist
led2 = 1;
wait(0.5);
led3 = 1;
@@ -90,6 +93,7 @@
EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
pc.printf("Initialisation abgeschlossen\n");
+ wait(0.1);
MyEpos1.Reset();
MyEpos2.Reset();
@@ -153,69 +157,107 @@
*/
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
+ ErrorState = MyEpos5.GetError();
+ wait(0.1);
+ pc.printf("ErrorState: %d\r\n", ErrorState);
}
switch (current_step){
case 1://HOMING:
+
led1 = 1;
-
- MyEpos1.Power(1);
- MyEpos2.Power(1);
- MyEpos3.Power(1);
- MyEpos4.Power(1);
- MyEpos5.Power(1);
- MyEpos6.Power(1);
- MyEpos7.Power(1);
+ if (Power_ein == true){
+ MyEpos1.Power(1);
+ MyEpos2.Power(1);
+ MyEpos3.Power(1);
+ MyEpos4.Power(1);
+ MyEpos5.Power(1);
+ MyEpos6.Power(1);
+ MyEpos7.Power(1);
+ Power_ein = false;
+ }
/***********************HOMING*************************/
+
if (node5_homed == false){
+ pc.printf("Homing5\n");
MyEpos5.Homing(); //Kopf
+ //wait(5);
+ MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
+ pc.printf("Fahren auf -60000\n");
+ while(MyEpos5.TargetReached()!=1){
+ }
+ pc.printf("Target reached\n");
node5_homed = true;
break;
}
-
+
if (node1_homed == false){
+ pc.printf("Homing1\n");
MyEpos1.Homing(); //Solarpanel_Aussen_Links
node1_homed = true;
break;
}
+
if (node3_homed == false){
+ pc.printf("Homing3\n");
MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
node3_homed = true;
break;
}
+
if (node7_homed == false){
+ pc.printf("Homing7\n");
MyEpos7.Homing(); //Bohrer
node7_homed = true;
break;
}
- if (node1_homed == true and node2_homed == false and node5_homed == true){
- MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
+ if (node1_homed == true and node2_homed == false){
+ pc.printf("Homing2\n");
MyEpos2.Homing(); //Solarpanel_Innen_Links
- MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
- wait(2);
node2_homed = true;
break;
}
if (node3_homed == true and node4_homed == false){
+ pc.printf("Homing4\n");
MyEpos4.Homing(); //Solarpanel_Innen_Rechts
node4_homed = true;
+ wait(0.1);
break;
}
+
+
if (node7_homed == true and node6_homed == false){
+ pc.printf("Homing6\n");
MyEpos6.Homing();//Bohrgestell
node6_homed = true;
break;
}
+
+ if (node2_homed==1 and node4_homed==1 and node5_grundposition == false){
+ MyEpos5.MoveAbsolute(0,800,100,100); //Kopf
+ wait(0.1);
+ while(MyEpos5.TargetReached() != 1){
+ wait(0.1);
+ }
+ node5_grundposition = true;
+ 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){
+ pc.printf("Homing beendet\n");
+ node5_grundposition == false;
+ Power_ein == true;
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
@@ -224,6 +266,7 @@
MyEpos5.Power(0);
current_step = 2;//BEREIT;
}
+
@@ -233,6 +276,7 @@
case 2://BEREIT:
+ //pc.printf("Bereit\n");
led2 = 1;
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
@@ -243,6 +287,7 @@
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
+ einfahren = false;
current_step = 6;
wait(0.5);
}
@@ -257,15 +302,16 @@
case 5://USV:
+ pc.printf("USV\n");
led4 = 1;
MyEpos5.SetDigOut(4,1);
switch (usv_step){
case 1:
- wait(0.1);
+// wait(0.1);
// 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);
+// wait(0.1);
// }
// if (node3_homed == true){
// MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
@@ -276,27 +322,28 @@
MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
wait(0.1);
}
- */ wait(0.1);
+ */// wait(0.1);
// usv_step = 2;
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
usv_step = 2;
+ wait(0.1);
}
break;
case 2:
- if (MyEpos1.TargetReached()==1){
- wait(1);
+// if (MyEpos1.TargetReached()==1){
+// wait(1);
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
- wait_ms(10);
- }
- if (MyEpos3.TargetReached()==1){
- wait(1);
+// 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);
+// wait_ms(10);
+// }
+// wait(0.1);
/* if (node4_homed == true and MyEpos3.TargetReached()==1){
wait(1);
MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
@@ -307,9 +354,10 @@
MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer
wait_ms(10);
}
-*/ wait(1);
+*/ wait(0.1);
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
usv_step = 3;
+ wait(0.1);
}
break;
@@ -326,7 +374,7 @@
/*while (MyEpos5.TargetReached()==0){
wait_ms(10);
}*/
- MyEpos1.Power(0);
+/* MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
@@ -334,7 +382,7 @@
MyEpos6.Power(0);
MyEpos7.Power(0);
usv_step = 5;
- MyEpos1.Power(0);
+*/ MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
@@ -342,6 +390,7 @@
MyEpos6.Power(0);
MyEpos7.Power(0);
usv_step = 5;
+ wait(0.1);
break;
case 5:
@@ -365,6 +414,7 @@
break;
case 6://Test
+ // pc.printf("Läuft...\n");
led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
@@ -402,9 +452,9 @@
wait(1);
}
break;
-
- }
- }
+
+ }//switch (ausfahren_step)
+ }//if (einfahren == false)
if (einfahren == true){
ausfahren_step = 1;
@@ -415,30 +465,31 @@
*/ MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
einfahren_step = 2;
- wait(1);
+ wait(0.1);
break;
case 2:
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
- wait(1);
+// wait(0.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 Innen Links
MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
einfahren_step = 3;
- wait(1);
+ wait(0.1);
}
break;
case 3:
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
- wait(1);
+// wait(1);
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
einfahren_step = 4;
+ wait(0.1);
}
break;
@@ -451,10 +502,51 @@
}
break;
- }
- }
+ }//switch(einfahren_step)
+ }//if einfahren == true)
led3 = 0;
break;
- }//case
+
+ case 7:
+ pc.printf("Fehler RESET\n");
+// if(MyEpos5.GetDigIn(3) == 1){
+ MyEpos1.MoveVelocity(0, 500, 500);
+ MyEpos2.MoveVelocity(0, 500, 500);
+ MyEpos3.MoveVelocity(0, 500, 500);
+ MyEpos4.MoveVelocity(0, 500, 500);
+ MyEpos5.MoveVelocity(0, 500, 500);
+ wait(3);
+ MyEpos1.Reset();
+ MyEpos1.Power(1);
+ MyEpos2.Reset();
+ MyEpos2.Power(1);
+ MyEpos3.Reset();
+ MyEpos3.Power(1);
+ MyEpos4.Reset();
+ MyEpos4.Power(1);
+ MyEpos5.Reset();
+ MyEpos5.Power(1);
+ node1_homed = false;
+ node2_homed = false;
+ node3_homed = false;
+ node4_homed = false;
+ node5_homed = false;
+ current_step = 1;
+ wait(0.1);
+// }
+
+// MyEpos5.MoveVelocity(0,500,500);
+ wait(0.1);
+
+/* MyEpos1.Reset();
+ MyEpos2.Reset();
+ MyEpos3.Reset();
+ MyEpos4.Reset();
+ MyEpos5.Reset();
+ wait(0.5);
+ */
+
+ break;
+ }//switch (current_step)
}//while
}//main