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:
- 10:1c42bbd596dc
- Parent:
- 9:039318b9096e
- Child:
- 11:6e5ebe9aa84b
--- a/main.cpp Fri Oct 14 13:41:28 2016 +0000
+++ b/main.cpp Mon Oct 17 05:39:38 2016 +0000
@@ -9,6 +9,24 @@
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);
@@ -45,15 +63,18 @@
led1 = 1;
pc.baud(9600);
+ buffering.rise(&interrupt_usv);
buffering.rise(&interrupt_usv); //Interrupt für USV
- wait(0.5); //Wartezeit bis Epos2 24/5 aufgestartet ist mit Lade-LED
+ 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);
+ wait(0.5);
+
+ pc.printf("Initialisation CAN\n");
can.frequency(1000000); //Define Can baud in bit/s
@@ -85,6 +106,51 @@
led4 = 0;
MyEpos5.SetDigOut(4, 1);
+/* usv_ready.mode(PullUp);
+ buffering.mode(PullUp);
+ replace_battery.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){
@@ -153,9 +219,10 @@
MyEpos4.Power(0);
wait(0.1);
MyEpos5.Power(0);
- MyEpos6.Power(0);
- MyEpos7.Power(0);
}
+
+
+
led1 = 0;
break;
@@ -166,22 +233,128 @@
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
MyEpos5.SetDigOut(4,1);
- current_step = 3; //ABLAUF
+ current_step = 6;
wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
// MyEpos4.Power(1);
MyEpos5.Power(1);
-// MyEpos6.Power(1);
-// MyEpos7.Power(1);
}
- wait(1);
+ wait_ms(10);
led2 = 0;
break;
- case 3://ABLAUF:
+ case 3://START_AUSFAHREN:
+
+ break;
+
+
+ case 5://USV:
+ led4 = 1;
+ MyEpos5.SetDigOut(4,1);
+ switch (usv_step){
+ case 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);
+ }
+ if (node3_homed == true){
+ MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
+ 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);
+ usv_step = 2;
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ usv_step = 2;
+ }
+ break;
+
+ case 2:
+ if (node2_homed == true and MyEpos1.TargetReached()==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,500,500); //Panel_Innen_Links
+ 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
+ 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:
+ /*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;
+ 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
led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
@@ -196,8 +369,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;
@@ -218,13 +391,16 @@
wait(1);
}
break;
- }//switch ausfahren_step
- }//if (einfahren == false)
+
+ }
+ }
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
MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
einfahren_step = 2;
@@ -233,7 +409,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
@@ -261,94 +437,12 @@
wait(1);
MyEpos5.Power(0);
}
- break;
- }//switch einfahren_step
- }//if (einfahren == true)
+ break;
+
+ }
+ }
led3 = 0;
break;
-
- 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
+ }//case
}//while
- }//main
\ No newline at end of file
+ }//main