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
main.cpp
- Committer:
- ODEM
- Date:
- 2016-09-22
- Revision:
- 4:8fb30d6c2855
- Parent:
- 3:fbc9de097e4f
- Child:
- 5:524e83ec5174
File content as of revision 4:8fb30d6c2855:
#include "mbed.h"
#include "EPOS2.h"
//***********************************************************************************************************************************//
//*** Global_Var *******************************************************************************************************************//
//***********************************************************************************************************************************//
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;
*/
int ausfahren = false;
int einfahren = false;
int current_step = 1;
int main(){ ///////// Main
pc.baud(9600);
wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
pc.printf("Initialisation CAN\n");
can.frequency(1000000); //Define Can baud in bit/s
CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
canOpen.start(); //Start defined CanOpen Network
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){
switch (current_step){
case 1://HOMING:
MyEpos1.Homing(); //Solarpanel_Aussen_Links
MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
MyEpos7.Homing(); //Bohrer
while (MyEpos2.GetDigIn(4)!=1 or MyEpos4.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);
}
MyEpos6.Homing();//Bohrgestell
while (MyEpos1.GetDigIn(4)!=1 or MyEpos3.GetDigIn(4)!=1) { //warten solange Solarpanels Innen NICHT in Grundposition
wait(0.1);
}
MyEpos5.Homing();//Kopf
while (MyEpos5.GetDigIn(4)!=1) { //warten solange Kopf NICHT 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_AUSFAHREN
break;
case 3://START_AUSFAHREN:
while(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
}
}
}
}
}
}
break;
case 4://Start_EINFAHREN
while(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
}
}
}
}
}
}
break;
case 5://USV:
while (MyEpos1.GetDigIn()){
MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
MyEpos7.MoveAbsolute(0,200,500,500); // Bohrer
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 (MyEpos7.TargetReached()==1){
MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
}
}
break;
case 6://ERROR2
break;
}//case
}//while
}//main