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:
- 2:0f4e77a22fd9
- Parent:
- 1:c53aafa72b36
- Child:
- 3:fbc9de097e4f
--- a/main.cpp Thu Jul 28 11:50:16 2016 +0000
+++ b/main.cpp Wed Sep 21 06:52:06 2016 +0000
@@ -1,6 +1,6 @@
#include "mbed.h"
#include "EPOS2.h"
-//#include "RobotControl.h"
+
//***********************************************************************************************************************************//
//*** Global_Var *******************************************************************************************************************//
@@ -11,7 +11,7 @@
CAN can(p9, p10); // Can Pin def
-
+/*
int velocity = 3000;
int acceleration = 50000;
int deceleration = 50000;
@@ -25,80 +25,12 @@
int counter=0;
int counter2=0;
bool start = true;
-int current_State = 2;
+int current_step = 1;
int counter_s = false;
int counter_s2 = false;
-
-
-
-void Motor1 (EPOS2 MyEpos){
- pc.printf("Motor1\n");
-
-
- MyEpos.SetDigOut(3, 0);
- wait(2);
-
-
-
-
- // while(1){
-
-
- // 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());
-
- while (MyEpos.TargetReached() == 0) {
- ActualPos = MyEpos.GetActualPos();
- DemandPos = MyEpos.GetDemandPos();
- ActualCurrent = MyEpos.GetActualCurrentAveraged();
- DemandCurrent = MyEpos.GetDemandCurrent();
- //wait_ms(100);
- MyEpos.SetDigOut(3, 0);
- } // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
-
-
- wait(0.5);
-
- MyEpos.SetDigOut(3, 1);
- //wait_ms(100);
- MyEpos.MoveAbsolute(0,3000,3000,3000);
- MyEpos.SetDigOut(3, 0);
- wait(1);
-
-
- while (MyEpos.TargetReached() == 0) {
- ActualPos = MyEpos.GetActualPos();
- DemandPos = MyEpos.GetDemandPos();
- ActualCurrent = MyEpos.GetActualCurrent();
- DemandCurrent = MyEpos.GetDemandCurrent();
- //wait_ms(100);
- MyEpos.SetDigOut(3, 0);
- //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
-
- }
- counter=0;
- //}
-}
-
-
-void Motor2(EPOS2 MyEpos2){
- pc.printf("Motor2\n");
-
-
-
- wait_ms(100) ;
- MyEpos2. MoveVelocity(1000, 80, 1000);
- wait(0.1);
-
-
-
-}
-int main() ///////// Main
-{
+*/
+
+int main(){ ///////// Main
pc.baud(9600);
wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
@@ -110,120 +42,92 @@
CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
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)
-
-
+ EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+ EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+ EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+ EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+ EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+ EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
+ EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
pc.printf("Initialisation abgeschlossen\n");
+
+ //************** Initialisierung **************//
+
+ MyEpos1.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos1.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos1.Reset();
+
+ MyEpos2.SetPar(10,2660, 1, 3100, 2.76); //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();
+
+ MyEpos3.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos3.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos3.Reset();
+
+ MyEpos4.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos4.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos4.Reset();
+
+ MyEpos5.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos5.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos5.Reset();
+
+ MyEpos6.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
+ MyEpos6.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ 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, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
+ MyEpos7.Reset();
while(1){
-
-
-
- //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");
-
- current_State = 3;//ERROR;
-
- }
- if (MyEpos2.GetError()==true&& counter2<3) {
- pc.printf("FEHLER-Node2!!!\n");
-
- 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;
-
-
+ switch (current_step){
+ case 1://HOMING:
+ MyEpos2.Homing(); //Solarpanel_Aussen_Links
+ MyEpos4.Homing(); //Solarpanel_Aussen_Rechts
+ MyEpos7.Homing(); //Bohrer
+ while (MyEpos2.GetDigIn(4)!1) or (MyEpos4.GetDigIn(4)!1){ //Kontrolle ob: Solarpanels Aussen in Grundposition
+ wait(0.1);
+ }
+ MyEpos1.Homing(); //Solarpanel_Innen_Links
+ MyEpos3.Homing(); //Solarpanel_Innen_Rechts
+
+ while (MyEpos7.GetDigIn(4)!1){ //Kontrolle ob: Bohrer eingefahren
+ wait(0.1);
+ }
+ MyEpos6.Homing();//Bohrgestell
+ while (MyEpos1.GetDigIn(4)!1) or (MyEpos3.GetDigIn(4)!1) { //Kontrolle ob: Solarpanels Innen in Grundposition
+ wait(0.1);
+ }
+ MyEpos5.Homing();//Kopf
+ while (MyEpos5.GetDigIn(4)!1) { //Kontrolle ob: Kopf in Grundposition
+ wait(0.1);
+ }
+ current_step = 2;//BEREIT;
+ break;
+
+
+ case 2://BEREIT:
+ while (MyEpos6.GetDigIn(1)!1) { //Kontrolle ob: Kopf in Grundposition
+ wait_ms(10);
+ }
+ current_step = 3;//START
+ break;
- 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:
-
+
+ case 3://START:
+ MyEpos2.MoveAbsolute(500,20,500,500);
+ MyEpos4.MoveAbsolute(500,20,500,500);
+ break;
-
- if (counter_s==false){
- counter++;
- counter_s=true;
-
- current_State = 2;// INITIALISIERUNG;
- }
-
- break;
-
- case 4: //STATE_OFF:
+
+ case 4://STATE_OFF:
+ break;
- MyEpos.Power(0);
- MyEpos2.Power(0);
-
- break;
-
- case 5://ERROR2
- if (counter_s2==false){
- counter2++;
- counter_s2=true;
-
- current_State = 2;// INITIALISIERUNG;
- }
-
+ case 5://ERROR2
break;
- }
-
-
- // pc.printf("%d\n", MyEpos.GetError());
- }//while
-}//main
\ No newline at end of file
+ }//case
+ }//while
+ }//main
\ No newline at end of file