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:
- 1:c53aafa72b36
- Parent:
- 0:aaf583a75b97
- Child:
- 2:0f4e77a22fd9
--- a/main.cpp Thu Jul 28 06:24:33 2016 +0000
+++ b/main.cpp Thu Jul 28 11:50:16 2016 +0000
@@ -23,8 +23,11 @@
int ActualCurrent = 0;
int DemandCurrent = 0;
int counter=0;
+int counter2=0;
bool start = true;
-bool counter_s =false;
+int current_State = 2;
+int counter_s = false;
+int counter_s2 = false;
@@ -41,12 +44,12 @@
// while(1){
- // pc.printf("start\n");
+ // pc.printf("start\n");
MyEpos.SetDigOut(3, 1); //(1=Enable, 0=Disable)
MyEpos.MoveAbsolute(500,20,500,deceleration);
MyEpos.SetDigOut(3, 0); //(TargetPosition, PosSpeed, acceleration, deceleration)
wait(1);
- // pc.printf("%d\n", MyEpos.GetError());
+ // pc.printf("%d\n", MyEpos.GetError());
while (MyEpos.TargetReached() == 0) {
ActualPos = MyEpos.GetActualPos();
@@ -77,6 +80,7 @@
//pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
}
+ counter=0;
//}
}
@@ -91,15 +95,12 @@
wait(0.1);
-
+
}
int main() ///////// Main
{
-restart:
- //************** Initialisierung **************//
- counter_s=false;
pc.baud(9600);
- pc.printf("Fehlerfall %d\n", counter) ;
+
wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
pc.printf("Initialisation CAN\n");
@@ -110,54 +111,119 @@
canOpen.start(); //Start defined CanOpen Network
EPOS2 MyEpos(&canOpen, 1);
- EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
-
- //RobotControl robotControl(&MyEpos, &MyEpos2);
- //robotControl.start();
-
-
- MyEpos.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
- MyEpos.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
- MyEpos.Reset(); //Reset the EPOS2
- wait(2);
- MyEpos2.SetPar(10,936, 8, 1000, 29.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
- MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
- MyEpos2.Reset(); //Reset the EPOS2
- wait(2);
- pc.printf("Homing\n");
-
- MyEpos.Power(1);
- pc.printf("power\n");
- MyEpos2.Power(1); //Enable the Epos for Homing
- pc.printf("Power2\n");
- wait(2);
- MyEpos.Homing(); //do Homing and wait untill homing is done // MyEpos.Power(1);
- wait_ms(100);
+ EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+
pc.printf("Initialisation abgeschlossen\n");
while(1){
- if (MyEpos.GetError()==true || MyEpos2.GetError()==true && counter<3) {
- pc.printf("FEHLER!!!\n");
+
+
+ //if(counter>2){
+ // MyEpos.Power(0);
+ // MyEpos2.Power(0);
+ // }
+
+
+
+
+ switch (current_State){
+
+ case 1://STATE_NORMAL:
+
+ if (MyEpos.GetError()==true && counter<3) {
+ pc.printf("FEHLER-Node1!!!\n");
- if (counter_s==false){
- counter++;
- counter_s=true;
- }
- goto restart;
+ current_State = 3;//ERROR;
+
+ }
+ if (MyEpos2.GetError()==true&& counter2<3) {
+ pc.printf("FEHLER-Node2!!!\n");
- }
-
- if(counter>2){
- MyEpos.Power(0);
- MyEpos2.Power(0);
- }
+ current_State = 5;//ERROR;
+
+ }
+
+
+ if (counter2>2){
+ MyEpos2.Power(0);
+ MyEpos.Power(0);
+ }
+
+ if (counter>2){
+ MyEpos2.Power(0);
+ MyEpos.Power(0);
+ }
+ Motor2(MyEpos2);
+ Motor1(MyEpos);
+ break;
+ case 2:// INITIALISIERUNG:
+
+
+ //************** Initialisierung **************//
+ counter_s=false;
+ counter_s2=false;
+
+
+
+ pc.printf("Fehlerfall %d\n", counter) ;
+ pc.printf("Fehlerfall2 %d\n", counter2) ;
+ MyEpos.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos.Reset(); //Reset the EPOS2
+ wait(2);
+ MyEpos2.SetPar(10,936, 8, 1000, 29.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos2.Reset(); //Reset the EPOS2
+ wait(2);
+ pc.printf("Homing\n");
+
+ MyEpos.Power(1);
+ pc.printf("power\n");
+ MyEpos2.Power(1); //Enable the Epos for Homing
+ pc.printf("Power2\n");
+ wait(2);
+ MyEpos.Homing();
+
+
+ } //do Homing and wait untill homing is done // MyEpos.Power(1);
+ wait_ms(100);
+ current_State = 1;//STATE_NORMAL;
+ break;
+ case 3://ERROR1:
+
+
+
+ if (counter_s==false){
+ counter++;
+ counter_s=true;
+
+ current_State = 2;// INITIALISIERUNG;
+ }
+
+ break;
+
+ case 4: //STATE_OFF:
+
+ MyEpos.Power(0);
+ MyEpos2.Power(0);
+
+ break;
+
+ case 5://ERROR2
+ if (counter_s2==false){
+ counter2++;
+ counter_s2=true;
+
+ current_State = 2;// INITIALISIERUNG;
+ }
- Motor2(MyEpos2);
- Motor1(MyEpos);
-
+ break;
+ }
+
+
// pc.printf("%d\n", MyEpos.GetError());
}//while
}//main
\ No newline at end of file