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:
- 7:44f2f9fd2eeb
- Parent:
- 6:3eaa420dc2f5
- Child:
- 8:98d4028cdea6
--- a/main.cpp Thu Oct 13 11:53:56 2016 +0000
+++ b/main.cpp Fri Oct 14 08:33:11 2016 +0000
@@ -27,9 +27,9 @@
int counter_s = false;
int counter_s2 = false;
*/
-DigitalIn usv_ready(p26);
-InterruptIn buffering(p25);
-DigitalIn replace_battery(p24);
+DigitalIn usv_ready(p23);
+InterruptIn buffering(p24);
+DigitalIn replace_battery(p25);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
@@ -44,16 +44,6 @@
bool node6_homed = true;
bool node7_homed = true;
-bool ausfahren_step1 = false;
-bool ausfahren_step2 = false;
-bool ausfahren_step3 = false;
-bool ausfahren_step4 = false;
-
-bool einfahren_step1 = false;
-bool einfahren_step2 = false;
-bool einfahren_step3 = false;
-bool einfahren_step4 = false;
-
int homing = false;
int ready = false;
int ausfahren = false;
@@ -61,16 +51,27 @@
int usv = false;
int current_step = 1; //HOMING
int ausfahren_step = 1;
-int einfahren_step = 1;
+int einfahren_step = 1;
+int usv_step = 1;
void interrupt_usv(){
current_step = 5;
- }
+ wait_ms(10);
+ }
int main(){ ///////// Main
+ led1 = 1;
pc.baud(9600);
- wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
+ buffering.rise(&interrupt_usv);
+
+ wait(0.5); //Wartezeit bis Epos2 70/10 aufgestartet ist
+ led2 = 1;
+ wait(0.5);
+ led3 = 1;
+ wait(0.5);
+ led4 = 1;
+ wait(0.5);
pc.printf("Initialisation CAN\n");
@@ -98,6 +99,11 @@
MyEpos7.Reset();
wait(0.1);
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+
/* mySwitch1.mode(PullUp);
mySwitch2.mode(PullUp);
mySwitch3.mode(PullUp);
@@ -145,7 +151,6 @@
*/
while(1){
- led4 = 1;
switch (current_step){
case 1://HOMING:
led1 = 1;
@@ -160,6 +165,12 @@
/***********************HOMING*************************/
+ if (node5_homed == false){
+ MyEpos5.Homing(); //Kopf
+ node5_homed = true;
+ break;
+ }
+
if (node1_homed == false){
MyEpos1.Homing(); //Solarpanel_Aussen_Links
node1_homed = true;
@@ -178,8 +189,11 @@
break;
}
- if (node1_homed == true and node2_homed == false){
+ if (node1_homed == true and node2_homed == false and node5_homed == true){
+ MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
MyEpos2.Homing(); //Solarpanel_Innen_Links
+ MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
+ wait(3);
node2_homed = true;
break;
}
@@ -190,12 +204,6 @@
break;
}
- if (node2_homed == true and node4_homed == true and node5_homed == false){
- MyEpos5.Homing();
- node5_homed = true;
- break;
- }
-
if (node7_homed == true and node6_homed == false){
MyEpos6.Homing();//Bohrgestell
node6_homed = true;
@@ -203,13 +211,15 @@
}
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);
}
- MyEpos1.Power(0);
- MyEpos2.Power(0);
- MyEpos3.Power(0);
- MyEpos4.Power(0);
- MyEpos5.Power(0);
+
led1 = 0;
break;
@@ -219,6 +229,7 @@
led2 = 1;
if (MyEpos5.GetDigIn(3) == 1){
current_step = 6;
+ wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
@@ -235,7 +246,7 @@
break;
- case 3://START_AUSFAHREN:
+/* case 3://START_AUSFAHREN:
led3 = 1;
if(ausfahren == true){
MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
@@ -288,27 +299,105 @@
led4 = 0;
break;
-
+ */
case 5://USV:
- while (MyEpos1.GetDigIn(1)){
- MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
- MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
- MyEpos7.MoveAbsolute(0,200,500,500); // Bohrer
- if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
- MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
- MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
- if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
- MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
+ led4 = 1;
+ switch (usv_step){
+ case 1:
+ wait(0.1);
+/* MyEpos1.Power(1);
+ MyEpos2.Power(1);
+ MyEpos3.Power(1);
+ MyEpos4.Power(1);
+ MyEpos5.Power(1);
+ MyEpos6.Power(1);
+ MyEpos7.Power(1);
+ wait(1);
+*/
+ if (node1_homed == true){
+ MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links
+ wait(0.1);
+ }
+ if (node3_homed == true){
+ MyEpos3.MoveAbsolute(0,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;
+ break;
+
+ case 2:
+ if (node2_homed == true and MyEpos1.TargetReached()==1){
+ wait(1);
+ MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+ wait_ms(10);
+ }
+/* if (node4_homed == true and MyEpos3.TargetReached()==1){
+ wait(1);
+ MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+ wait_ms(10);
+ }
+ if (node6_homed == true and MyEpos7.TargetReached()==1){
+ wait(1);
+ MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer
+ wait_ms(10);
}
- }
- if (MyEpos7.TargetReached()==1){
- MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
+*/ wait(1);
+ if (MyEpos2.TargetReached()==1 /*and MyEpos4.TargetReached()==1 and MyEpos6.TargetReached()==1*/){
+ usv_step = 3;
}
+ break;
+
+ case 3:
+ MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
+ wait(0.1);
+ if (MyEpos5.TargetReached()==1){
+ usv_step = 4;
+ wait(0.1);
+ }
+ break;
+
+ case 4:
+ /*while (MyEpos5.TargetReached()==0){
+ wait_ms(10);
+ }*/
+ MyEpos1.Power(0);
+ MyEpos2.Power(0);
+ MyEpos3.Power(0);
+ MyEpos4.Power(0);
+ MyEpos5.Power(0);
+ MyEpos6.Power(0);
+ MyEpos7.Power(0);
+ usv_step = 5;
+ break;
+
+ case 5:
+ if(buffering == 1){
+ led3 = 1;
+ } else {
+ led3 = 0;
+ current_step=1;
+ usv_step=1;
+ ausfahren_step=1;
+ einfahren_step=1;
+ }
+ /*if (buffering == 0){
+ current_step = 1;
+ usv_step = 1;
+ }*/
+ wait(1);
+ break;
}
+ led4 = 0;
break;
case 6://Test
// MyEpos5.Power(1);
+ led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
case 1:
@@ -319,7 +408,8 @@
case 2:
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
ausfahren_step = 3;
wait(1);
@@ -389,8 +479,8 @@
}
}
+ led3 = 0;
break;
- }//case
- led4 = 0;
+ }//case
}//while
}//main
\ No newline at end of file