mmotor
Programm mit Ablauf
Dependencies: EPOS2 mbed-rtos mbed
Fork of
Projekt_Kugelbahn
by 
mmotor
Diff: main.cpp
- Revision:
- 14:bee23de5dff0
- Parent:
- 13:cc43e546f310
- Child:
- 15:c31c862632c0
- Child:
- 16:e5343d6c9fb2
--- a/main.cpp Wed Oct 19 10:02:02 2016 +0000
+++ b/main.cpp Thu Nov 03 10:36:46 2016 +0000
@@ -27,8 +27,8 @@
bool node3_homed = false;
bool node4_homed = false;
bool node5_homed = false;
-bool node6_homed = true;
-bool node7_homed = true;
+bool node6_homed = false;
+bool node7_homed = false;
bool Power_ein = true;
bool node5_grundposition = false;
@@ -47,7 +47,7 @@
int ErrorState5;
int ErrorState6;
int ErrorState7;
-float speed = 1.2;
+float speed = 1.5;
void interrupt_usv(){ //Funktion, wird aufgeruffen durch den interrupt des USV's
current_step = 5;
@@ -106,6 +106,12 @@
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
+ 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(0.1);
ErrorState1 = MyEpos1.GetError();
ErrorState2 = MyEpos2.GetError();
ErrorState3 = MyEpos3.GetError();
@@ -115,15 +121,19 @@
pc.printf("ErrorState Panels/Kopf: Panel Aussen Linkss=%d, Panel Innen Links=%d, Panel Aussen Rechts=%d, Panel Innen Rechts=%d, Kopf=%d\r\n", ErrorState1, ErrorState2, ErrorState3, ErrorState4, ErrorState5);
}
-/* if(MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0){
- current_step = 76; //FEHLER
+ if(MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0){
+ current_step = 6; //FEHLER
+ MyEpos1.MoveVelocity(0, 500, 500);
+ MyEpos2.MoveVelocity(0, 500, 500);
+ MyEpos3.MoveVelocity(0, 500, 500);
+ wait(0.1);
ErrorState6 = MyEpos6.GetError();
ErrorState7 = MyEpos7.GetError();
wait(0.1);
pc.printf("Bohrgestell/Bohrer ErrorState: Bohrgestell=%d, Bohrer=%d\r\n", ErrorState6, ErrorState7);
}
-*/ switch (current_step){
+ switch (current_step){
case 1://HOMING:
led1 = 1;
@@ -140,33 +150,12 @@
/***********************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){
- if(MyEpos5.GetError() == 1){
- break;
- }
- }
- pc.printf("Target reached\n");
- node5_homed = true;
- break;
- }
-
if (node1_homed == false){
if(epos1_homing == false){
MyEpos1.MoveVelocity(-400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
epos1_homing = true;
}
-/* if(MyEpos1.GetError() != 0){
- current_step = 7;
- break;
- }
-*/ if(MyEpos1.GetDigIn(4) == 1){
+ if(MyEpos1.GetDigIn(4) == 1){
pc.printf("Homing1\n");
MyEpos1.Homing(); //Solarpanel_Aussen_Links
node1_homed = true;
@@ -181,19 +170,29 @@
MyEpos3.MoveVelocity(400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
epos3_homing = true;
}
-/* if(MyEpos3.GetError() != 0){
- current_step = 7;
- break;
- }
-*/ if(MyEpos3.GetDigIn(4) == 1){
+ if(MyEpos3.GetDigIn(4) == 1){
pc.printf("Homing3\n");
MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
node3_homed = true;
}
break;
}
-
+ if (node5_homed == false){
+ pc.printf("Homing5\n");
+ MyEpos5.Homing(); //Kopf
+ MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
+ pc.printf("Fahren auf -60000\n");
+ while(MyEpos5.TargetReached()!=1){
+ if(MyEpos5.GetError() == 1){
+ break;
+ }
+ }
+ pc.printf("Target reached\n");
+ node5_homed = true;
+ break;
+ }
+
if (node7_homed == false){
pc.printf("Homing7\n");
MyEpos7.Homing(); //Bohrer
@@ -215,6 +214,7 @@
pc.printf("Homing4\n");
MyEpos4.Homing(); //Solarpanel_Innen_Rechts
node4_homed = true;
+ MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
wait(0.1);
break;
}
@@ -229,7 +229,7 @@
}
if (node2_homed == true and node4_homed == true and node5_grundposition == false){
- MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
+ //MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
wait(0.1);
while(MyEpos5.TargetReached() != 1){
if(MyEpos5.GetError() == 1){
@@ -260,7 +260,6 @@
case 2://BEREIT:
- //pc.printf("Bereit\n");
led2 = 1;
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
@@ -271,12 +270,28 @@
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
+ MyEpos6.Power(1);
+ MyEpos7.Power(1);
einfahren = false;
ausfahren_step = 1;
current_step = 3;
wait(0.5);
}
+ wait(0.1);
+ /********TEST********/ /*
+ MyEpos5.SetDigOut(4,1);
wait_ms(10);
+ MyEpos1.Power(1);
+ MyEpos2.Power(1);
+ MyEpos3.Power(1);
+ MyEpos4.Power(1);
+ MyEpos5.Power(1);
+ MyEpos6.Power(1);
+ MyEpos7.Power(1);
+ einfahren = false;
+ ausfahren_step = 1;
+ current_step = 3;
+ */ /********TEST*******/
led2 = 0;
break;
@@ -300,6 +315,7 @@
ausfahren_step = 3;
wait(1);
}
+ wait(0.1);
break;
case 3:
@@ -310,15 +326,50 @@
ausfahren_step = 4;
wait(1);
}
+ wait(0.1);
break;
case 4:
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ MyEpos6.MoveAbsolute(-76000,500*speed,100*speed,100*speed); //Bohrgestell
+ ausfahren_step = 5;
+ wait(1);
+ }
+ wait(0.1);
+ break;
+
+
+ case 5:
+ if (MyEpos6.TargetReached()==1){
+ MyEpos7.MoveAbsolute(700000,2000*speed,500*speed,500*speed); //Bohrer
+ ausfahren_step = 6;
+ wait(1);
+ }
+ wait(0.1);
+ break;
+
+
+ case 6:
+ if (MyEpos7.TargetReached()==1){
einfahren = true;
einfahren_step = 1;
wait(1);
}
+ wait(0.1);
+// while(buffering == 0){//für Fotograf
+// }
break;
+
+ /*
+ case 4:
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ einfahren = true;
+ einfahren_step = 3;
+ wait(1);
+ }
+ wait(0.1);
+ break;
+ */
@@ -328,43 +379,65 @@
if (einfahren == true){
ausfahren_step = 1;
switch (einfahren_step){
+
case 1:
- MyEpos1.MoveAbsolute(4000,500*speed,100*speed,100*speed); //Panel Aussen Links
- MyEpos3.MoveAbsolute(-4000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
+ MyEpos7.MoveAbsolute(0,2000*speed,500*speed,500*speed); //Bohrer
einfahren_step = 2;
wait(0.1);
break;
+
+ case 2:
+ if (MyEpos7.TargetReached()==1){
+ MyEpos6.MoveAbsolute(0,500*speed,100*speed,100*speed); //Bohrgestell
+ einfahren_step = 3;
+ wait(1);
+ }
+ wait(0.1);
+ break;
- case 2:
+ case 3:
+// if (MyEpos7.TargetReached()==1){
+ MyEpos1.MoveAbsolute(10000,500*speed,100*speed,100*speed); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(-10000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
+ einfahren_step = 4;
+// wait(1);
+// }
+ wait(0.1);
+ break;
+
+ case 4:
if (MyEpos1.GetActualPos() < 20000 and MyEpos3.GetActualPos() > -20000){
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
MyEpos2.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Links
MyEpos4.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Rechts
- einfahren_step = 3;
+ einfahren_step = 5;
wait(0.1);
}
+ wait(0.1);
break;
- case 3:
+ case 5:
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
MyEpos5.MoveAbsolute(1000,500*speed,100*speed,100*speed); //Kopf
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
- einfahren_step = 4;
+ einfahren_step = 6;
wait(0.1);
}
+ wait(0.1);
break;
- case 4:
+ case 6:
if (MyEpos5.TargetReached()==1){
einfahren = false;
current_step = 2;
wait(1);
MyEpos5.Power(0);
}
+ wait(0.1);
break;
}//switch(einfahren_step)
@@ -378,6 +451,8 @@
MyEpos5.SetDigOut(4,1);
switch (usv_step){
case 0:
+ pc.printf("USV 0\n");
+ wait(0.1);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
@@ -386,26 +461,31 @@
MyEpos6.Power(1);
MyEpos7.Power(1);
usv_step = 1;
+ wait(0.1);
break;
case 1:
+ pc.printf("USV 1\n");
MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
+ MyEpos7.MoveAbsolute(0,2000,500,500); //Bohrer
wait(0.1);
- if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1 and MyEpos7.TargetReached()==1){
usv_step = 2;
wait(0.1);
}
break;
case 2:
-
+ pc.printf("USV 2\n");
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+
+ MyEpos6.MoveAbsolute(0,500,100,100); //Bohrgestell
wait(0.1);
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
@@ -415,15 +495,17 @@
break;
case 3:
- MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
+ pc.printf("USV 3\n");
+ MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
+ wait(0.1);
+ if (MyEpos5.TargetReached()==1){
+ usv_step = 4;
wait(0.1);
- if (MyEpos5.TargetReached()==1){
- usv_step = 4;
- wait(0.1);
- }
+ }
break;
case 4:
+ pc.printf("USV 4\n");
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
@@ -436,6 +518,7 @@
break;
case 5:
+ pc.printf("USV 5\n");
if(buffering == 1){
led3 = 1;
} else {
@@ -445,6 +528,8 @@
usv_step=0;
ausfahren_step=1;
einfahren_step=1;
+ wait(0.1);
+ break;
}
wait(1);
break;
@@ -452,18 +537,22 @@
led4 = 0;
break;
- case 6:
+ case 6: //Fehler Bohrer/Bohrgestell
pc.printf("Fehler RESET Bohrer\n");
- MyEpos5.MoveVelocity(0, 500, 500);
- MyEpos6.MoveVelocity(0, 500, 500);
- wait(3);
- MyEpos5.Reset();
- MyEpos5.Power(1);
- MyEpos6.Reset();
- MyEpos6.Power(1);
+ MyEpos7.MoveVelocity(0, 500, 500);
+ if (MyEpos7.TargetReached() == 1){
+ MyEpos6.MoveVelocity(0, 500, 500);
+ if(MyEpos6.TargetReached() == 1){
+ MyEpos5.Reset();
+ MyEpos5.Power(1);
+ MyEpos6.Reset();
+ MyEpos6.Power(1);
+ current_step = 1;
+ }
+ }
//node1_homed = false;
//node2_homed = false;
- current_step = 3;
+ //current_step = 1;
break;
case 7:
@@ -473,7 +562,7 @@
MyEpos3.MoveVelocity(0, 500, 500);
MyEpos4.MoveVelocity(0, 500, 500);
MyEpos5.MoveVelocity(0, 500, 500);
- wait(3);
+ wait(1);
MyEpos1.Reset();
MyEpos1.Power(1);
MyEpos2.Reset();
@@ -493,17 +582,21 @@
node5_homed = false;
Power_ein = true;
if(MyEpos2.GetActualPos() < 100000 or MyEpos4.GetActualPos() > -100000){
- MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
- MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
+ MyEpos1.Homing();
+ MyEpos3.Homing();
+ MyEpos1.MoveAbsolute(0,500,100,100);
+ MyEpos3.MoveAbsolute(0,500,100,100);
MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
while (MyEpos2.TargetReached() != 1 or MyEpos4.TargetReached() != 1){
if(MyEpos2.GetError() == 1 or MyEpos4.GetError() == 1){
+ //wait(0.1);
break;
}
+ //wait(0.1);
}
}
- current_step = 1;
+ current_step = 6;
wait(0.1);
break;