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:
- 6:3eaa420dc2f5
- Parent:
- 5:524e83ec5174
- Child:
- 7:44f2f9fd2eeb
--- a/main.cpp Thu Sep 29 10:12:24 2016 +0000
+++ b/main.cpp Thu Oct 13 11:53:56 2016 +0000
@@ -27,15 +27,45 @@
int counter_s = false;
int counter_s2 = false;
*/
-DigitalIn mySwitch1(p21);
-DigitalIn mySwitch2(p22);
-DigitalIn mySwitch3(p23);
-/*mySwitch1.mode(PullUp);
-mySwitch2.mode(PullUp);
-mySwitch3.mode(PullUp);*/
+DigitalIn usv_ready(p26);
+InterruptIn buffering(p25);
+DigitalIn replace_battery(p24);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+bool node1_homed = false;
+bool node2_homed = false;
+bool node3_homed = false;
+bool node4_homed = true;
+bool node5_homed = false;
+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;
-int einfahren = false;
-int current_step = 1;
+int einfahren = false;
+int usv = false;
+int current_step = 1; //HOMING
+int ausfahren_step = 1;
+int einfahren_step = 1;
+
+void interrupt_usv(){
+ current_step = 5;
+ }
int main(){ ///////// Main
pc.baud(9600);
@@ -59,6 +89,19 @@
pc.printf("Initialisation abgeschlossen\n");
+ MyEpos1.Reset();
+ MyEpos2.Reset();
+ MyEpos3.Reset();
+ MyEpos4.Reset();
+ MyEpos5.Reset();
+ MyEpos6.Reset();
+ MyEpos7.Reset();
+ wait(0.1);
+
+ /* mySwitch1.mode(PullUp);
+ mySwitch2.mode(PullUp);
+ mySwitch3.mode(PullUp);
+ */
//************** Initialisierung **************//
/* HOMING METHODEN:
@@ -66,27 +109,30 @@
Homing Method 11:Home Switch Negative Speed & Index
Homing Method 23: Home Switch Positive Speed
Homing Method 27: Home Switch Negative Speed
+ Homing Method -3: Current Threshold Positive Speed
+ Homing Method -4: Current Threshold Negative Speed
+
*/
//Motortype 10: EC-Motor
- MyEpos1.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos1.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
+/* MyEpos1.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
+ MyEpos1.SetHomingPar(-4, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
MyEpos1.Reset();
- MyEpos2.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos2.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
+ MyEpos2.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
+ MyEpos2.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
MyEpos2.Reset();
- MyEpos3.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos3.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
+ MyEpos3.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
+ MyEpos3.SetHomingPar(-3, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
MyEpos3.Reset();
- MyEpos4.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
+ MyEpos4.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
MyEpos4.Reset();
- MyEpos5.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos5.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
+ MyEpos5.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
+ MyEpos5.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
MyEpos5.Reset();
MyEpos6.SetPar(10,3440, 1, 12000, 3.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
@@ -96,61 +142,102 @@
MyEpos7.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
MyEpos7.Reset();
+*/
while(1){
+ led4 = 1;
switch (current_step){
case 1://HOMING:
-
- MyEpos1.Reset();
- MyEpos2.Reset();
- MyEpos3.Reset();
- MyEpos4.Reset();
- MyEpos5.Reset();
- MyEpos6.Reset();
- MyEpos7.Reset();
+ led1 = 1;
-// MyEpos1.Power(1);
-// MyEpos2.Power(1);
+ MyEpos1.Power(1);
+ MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
MyEpos6.Power(1);
-// MyEpos7.Power(1);
-// MyEpos1.Homing(); //Solarpanel_Aussen_Links
-// MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
-// MyEpos7.Homing(); //Bohrer
-// while (/*MyEpos1.GetDigIn(4)!=1 or*/ MyEpos3.GetDigIn(4)!=1){ //warten solange Solarpanels Aussen NICHT in Grundposition
-// wait(0.1);
-// }
-// MyEpos2.Homing(); //Solarpanel_Innen_Links
- MyEpos4.Homing(); //Solarpanel_Innen_Rechts
-
-/* while (MyEpos7.GetDigIn(4)!=1){ //warten solange Bohrer NICHT eingefahren
- wait(0.1);
+ MyEpos7.Power(1);
+
+ /***********************HOMING*************************/
+
+ if (node1_homed == false){
+ MyEpos1.Homing(); //Solarpanel_Aussen_Links
+ node1_homed = true;
+ break;
+ }
+
+ if (node3_homed == false){
+ MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
+ node3_homed = true;
+ break;
+ }
+
+ if (node7_homed == false){
+ MyEpos7.Homing(); //Bohrer
+ node7_homed = true;
+ break;
+ }
+
+ if (node1_homed == true and node2_homed == false){
+ MyEpos2.Homing(); //Solarpanel_Innen_Links
+ node2_homed = true;
+ break;
}
- MyEpos6.Homing();//Bohrgestell
-*/ while (/*MyEpos2.GetDigIn(4)!=1 or */MyEpos4.GetDigIn(4)!=1) { //warten solange Solarpanels Innen NICHT in Grundposition
- wait(0.1);
+
+ if (node3_homed == true and node4_homed == false){
+ MyEpos4.Homing(); //Solarpanel_Innen_Rechts
+ node4_homed = true;
+ break;
+ }
+
+ if (node2_homed == true and node4_homed == true and node5_homed == false){
+ MyEpos5.Homing();
+ node5_homed = true;
+ break;
}
- MyEpos5.Homing();//Kopf
- while (MyEpos5.GetDigIn(4)!=1) { //warten solange Kopf NICHT in Grundposition
- wait(0.1);
+
+ if (node7_homed == true and node6_homed == false){
+ MyEpos6.Homing();//Bohrgestell
+ node6_homed = 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){
+ current_step = 2;//BEREIT;
}
- current_step = 2;//BEREIT;
+
+ MyEpos1.Power(0);
+ MyEpos2.Power(0);
+ MyEpos3.Power(0);
+ MyEpos4.Power(0);
+ MyEpos5.Power(0);
+
+ led1 = 0;
break;
case 2://BEREIT:
+ led2 = 1;
+ if (MyEpos5.GetDigIn(3) == 1){
+ current_step = 6;
+ MyEpos1.Power(1);
+ MyEpos2.Power(1);
+ MyEpos3.Power(1);
+ MyEpos4.Power(1);
+ MyEpos5.Power(1);
+ }
+ wait_ms(10);
/* while (MyEpos6.GetDigIn(1)!=1) { //warten solange Buzzer nicht gedrückt
wait_ms(10);
}
current_step = 3;//START_AUSFAHREN
-*/ current_step = 6; //Test
+ current_step = 6; //Test
+*/ led2 = 0;
break;
case 3://START_AUSFAHREN:
- while(ausfahren == true){
+ led3 = 1;
+ if(ausfahren == true){
MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
if (MyEpos5.TargetReached()==1){
MyEpos2.MoveAbsolute(500,200,500,500); //Panel_Innen_Links
@@ -172,11 +259,12 @@
}
}
}
-
+ led3 = 0;
break;
case 4://Start_EINFAHREN
- while(einfahren == true){
+ led4 = 1;
+ if(einfahren == true){
MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
if (MyEpos7.TargetReached()==1){
MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
@@ -197,7 +285,7 @@
}
}
}
-
+ led4 = 0;
break;
@@ -220,20 +308,89 @@
break;
case 6://Test
- MyEpos5.Power(1);
- wait(1);
- MyEpos5.MoveAbsolute(-151000,200,100,100); //Kopf
- wait_ms(10);
- while (MyEpos5.TargetReached()!=1){
- wait(1);
+// MyEpos5.Power(1);
+ if (einfahren == false){
+ switch (ausfahren_step){
+ case 1:
+ MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
+ ausfahren_step = 2;
+ wait(1);
+ break;
+
+ case 2:
+ if (MyEpos5.TargetReached()==1){
+
+ MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Aussen Links
+ ausfahren_step = 3;
+ wait(1);
+ }
+ break;
+
+ case 3:
+ if (MyEpos2.TargetReached()==1){
+ MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
+
+ ausfahren_step = 4;
+ wait(1);
+ }
+ break;
+
+ case 4:
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ einfahren = true;
+ einfahren_step = 1;
+ wait(1);
+ }
+ break;
+
+ }
}
- MyEpos5.MoveAbsolute(0,200,100,100); //Kopf
- wait_ms(10);
- while (MyEpos5.TargetReached()!=1){
- wait(1);
+
+ if (einfahren == true){
+ ausfahren_step = 1;
+ switch (einfahren_step){
+ case 1:
+ MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(-1000,500,100,100); //Panel Aussen Rechts
+ einfahren_step = 2;
+ wait(1);
+ break;
+
+ case 2:
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ wait(1);
+ MyEpos2.MoveAbsolute(0,500,100,100); //Panel Aussen Links
+ einfahren_step = 3;
+ wait(1);
+ }
+ break;
+
+ case 3:
+ if (MyEpos2.TargetReached()==1){
+ MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
+ wait(1);
+ MyEpos1.Power(0);
+ MyEpos2.Power(0);
+ MyEpos3.Power(0);
+ MyEpos4.Power(0);
+ einfahren_step = 4;
+ }
+ break;
+
+ case 4:
+ if (MyEpos5.TargetReached()==1){
+ einfahren = false;
+ current_step = 2;
+ wait(1);
+ MyEpos5.Power(0);
+ }
+ break;
+
+ }
}
- current_step = 2;//Bereit
break;
- }//case
+ }//case
+ led4 = 0;
}//while
}//main
\ No newline at end of file