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:
- 9:039318b9096e
- Parent:
- 8:98d4028cdea6
- Child:
- 10:1c42bbd596dc
--- a/main.cpp Fri Oct 14 11:17:52 2016 +0000
+++ b/main.cpp Fri Oct 14 13:41:28 2016 +0000
@@ -9,24 +9,6 @@
Serial pc(USBTX, USBRX); // (tx, rx)
CAN can(p9, p10); // Can Pin def
-/*
-int velocity = 3000;
-int acceleration = 50000;
-int deceleration = 50000;
-
-int anzahl_zyklen = 1000000;
-int absolvierte_zyklen = 0;
-int ActualPos = 0;
-int DemandPos = 0;
-int ActualCurrent = 0;
-int DemandCurrent = 0;
-int counter=0;
-int counter2=0;
-bool start = true;
-
-int counter_s = false;
-int counter_s2 = false;
-*/
DigitalIn usv_ready(p23);
InterruptIn buffering(p24);
DigitalIn replace_battery(p25);
@@ -65,15 +47,13 @@
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 mit Lade-LED
led2 = 1;
wait(0.5);
led3 = 1;
wait(0.5);
led4 = 1;
- wait(0.5);
-
- pc.printf("Initialisation CAN\n");
+ wait(0.5);
can.frequency(1000000); //Define Can baud in bit/s
@@ -105,51 +85,6 @@
led4 = 0;
MyEpos5.SetDigOut(4, 1);
- /* mySwitch1.mode(PullUp);
- mySwitch2.mode(PullUp);
- mySwitch3.mode(PullUp);
- */
- //************** Initialisierung **************//
-
- /* HOMING METHODEN:
- Homing Method 7 :Home Switch Positive Speed & Index
- 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, 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, 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, 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, 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, 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
- MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT
- MyEpos6.Reset();
-
- 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){
switch (current_step){
@@ -218,6 +153,8 @@
MyEpos4.Power(0);
wait(0.1);
MyEpos5.Power(0);
+ MyEpos6.Power(0);
+ MyEpos7.Power(0);
}
led1 = 0;
@@ -229,174 +166,22 @@
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
MyEpos5.SetDigOut(4,1);
- current_step = 6;
+ current_step = 3; //ABLAUF
wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
- MyEpos4.Power(1);
+// MyEpos4.Power(1);
MyEpos5.Power(1);
+// MyEpos6.Power(1);
+// MyEpos7.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
-*/ led2 = 0;
+ wait(1);
+ led2 = 0;
break;
-/* case 3://START_AUSFAHREN:
- 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
- MyEpos4.MoveAbsolute(500,200,500,500); //Panel_Innen_Rechts
- if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
- MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
- MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
- if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
- MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
- if (MyEpos6.TargetReached()==1){
- MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
- if (MyEpos7.TargetReached()==1){
- ausfahren = false;
- einfahren = true;
- current_step = 4;//START_EINFAHREN
- }
- }
- }
- }
- }
- }
- led3 = 0;
- break;
-
- case 4://Start_EINFAHREN
- led4 = 1;
- if(einfahren == true){
- MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
- if (MyEpos7.TargetReached()==1){
- MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
- if (MyEpos6.TargetReached()==1){
- MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
- MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
- 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
- if (MyEpos5.TargetReached()==1){
- einfahren = false;
- current_step = 2;//Bereit
- }
- }
- }
- }
- }
- }
- led4 = 0;
- break;
-
- */
- case 5://USV:
- led4 = 1;
- MyEpos5.SetDigOut(4,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(2000,500,500,500); //Panel_Aussen_Links
- wait(0.1);
- }
- if (node3_homed == true){
- 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);
- if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
- usv_step = 2;
- }
- break;
-
- case 2:
- 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(0.1);
-/* 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);
- }
-*/ 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:
- 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);
+ case 3://ABLAUF:
led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
@@ -411,8 +196,8 @@
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
+ wait(0.1);
ausfahren_step = 3;
- wait(1);
}
break;
@@ -433,9 +218,8 @@
wait(1);
}
break;
-
- }
- }
+ }//switch ausfahren_step
+ }//if (einfahren == false)
if (einfahren == true){
ausfahren_step = 1;
@@ -449,7 +233,7 @@
case 2:
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
- wait(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,100,100); //Panel Aussen Links
@@ -477,12 +261,94 @@
wait(1);
MyEpos5.Power(0);
}
- break;
-
- }
- }
+ break;
+ }//switch einfahren_step
+ }//if (einfahren == true)
led3 = 0;
break;
- }//case
+
+ case 5://USV:
+ led4 = 1;
+ MyEpos5.SetDigOut(4,1);
+ switch (usv_step){
+ case 1:
+ MyEpos1.Power(1);
+ MyEpos2.Power(1);
+ MyEpos3.Power(1);
+ MyEpos4.Power(1);
+ MyEpos5.Power(1);
+ MyEpos6.Power(1);
+ MyEpos7.Power(1);
+ wait(0.1);
+
+ MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
+ MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
+// MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
+ wait(0.1);
+ usv_step = 2;
+ break;
+
+ case 2:
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1 /*and MyEpos7.TargetReached()==1*/){
+ usv_step = 3;
+ wait_ms(10);
+ }
+ break;
+
+ case 3:
+ 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
+// MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+// MyEpos6.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+ wait(0.1);
+
+ if (MyEpos2.TargetReached()==1 /*and MyEpos4.TargetReached()==1*/){
+ usv_step = 4;
+ wait_ms(10);
+ }
+ break;
+
+ case 4:
+ MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
+ wait(0.1);
+ if (MyEpos5.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
+ usv_step = 5;
+ wait(0.1);
+ }
+ break;
+
+ case 5:
+ MyEpos1.Power(0);
+ MyEpos2.Power(0);
+ MyEpos3.Power(0);
+ MyEpos4.Power(0);
+ MyEpos5.Power(0);
+ MyEpos6.Power(0);
+ MyEpos7.Power(0);
+ usv_step = 6;
+ break;
+
+ case 6:
+ if(buffering == 1){
+ led3 = 1;
+ } else {
+ led3 = 0;
+ current_step=1;
+ usv_step=1;
+ ausfahren_step=1;
+ einfahren_step=1;
+ }
+ wait(1);
+ break;
+ }
+ led4 = 0;
+ break;
+
+
+ case 6://Test
+ break;
+
+ }//switch current_step
}//while
}//main
\ No newline at end of file