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:
- joe_feubli
- Date:
- 2016-10-14
- Revision:
- 8:98d4028cdea6
- Parent:
- 7:44f2f9fd2eeb
- Child:
- 9:039318b9096e
File content as of revision 8:98d4028cdea6:
#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;
*/
DigitalIn usv_ready(p23);
InterruptIn buffering(p24);
DigitalIn replace_battery(p25);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
bool node1_homed = false;
bool node2_homed = false;
bool node3_homed = false;
bool node4_homed = true;
bool node5_homed = false;
bool node6_homed = true;
bool node7_homed = true;
int homing = false;
int ready = false;
int ausfahren = false;
int einfahren = false;
int usv = false;
int current_step = 1; //HOMING
int ausfahren_step = 1;
int einfahren_step = 1;
int usv_step = 1;
void interrupt_usv(){
current_step = 5;
wait_ms(10);
}
int main(){ ///////// Main
led1 = 1;
pc.baud(9600);
buffering.rise(&interrupt_usv); //Interrupt für USV
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);
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");
MyEpos1.Reset();
MyEpos2.Reset();
MyEpos3.Reset();
MyEpos4.Reset();
MyEpos5.Reset();
MyEpos6.Reset();
MyEpos7.Reset();
wait(0.1);
led1 = 0;
led2 = 0;
led3 = 0;
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){
case 1://HOMING:
led1 = 1;
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
MyEpos6.Power(1);
MyEpos7.Power(1);
/***********************HOMING*************************/
if (node5_homed == false){
MyEpos5.Homing(); //Kopf
node5_homed = true;
break;
}
if (node1_homed == false){
MyEpos1.Homing(); //Solarpanel_Aussen_Links
node1_homed = true;
break;
}
if (node3_homed == false){
MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
node3_homed = true;
break;
}
if (node7_homed == false){
MyEpos7.Homing(); //Bohrer
node7_homed = true;
break;
}
if (node1_homed == true and node2_homed == false and node5_homed == true){
MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
MyEpos2.Homing(); //Solarpanel_Innen_Links
MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
wait(3);
node2_homed = true;
break;
}
if (node3_homed == true and node4_homed == false){
MyEpos4.Homing(); //Solarpanel_Innen_Rechts
node4_homed = true;
break;
}
if (node7_homed == true and node6_homed == false){
MyEpos6.Homing();//Bohrgestell
node6_homed = true;
break;
}
if (node1_homed == true and node2_homed == true and node3_homed == true and node4_homed == true and node5_homed == true and node6_homed == true and node7_homed == true){
current_step = 2;//BEREIT;
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
wait(0.1);
MyEpos5.Power(0);
}
led1 = 0;
break;
case 2://BEREIT:
led2 = 1;
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
MyEpos5.SetDigOut(4,1);
current_step = 6;
wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.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;
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);
led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
case 1:
MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
ausfahren_step = 2;
wait(1);
break;
case 2:
if (MyEpos5.TargetReached()==1){
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
ausfahren_step = 3;
wait(1);
}
break;
case 3:
if (MyEpos2.TargetReached()==1){
MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
ausfahren_step = 4;
wait(1);
}
break;
case 4:
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
einfahren = true;
einfahren_step = 1;
wait(1);
}
break;
}
}
if (einfahren == true){
ausfahren_step = 1;
switch (einfahren_step){
case 1:
MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
einfahren_step = 2;
wait(1);
break;
case 2:
if (MyEpos1.TargetReached()==1 and MyEpos3.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,100,100); //Panel Aussen Links
einfahren_step = 3;
wait(1);
}
break;
case 3:
if (MyEpos2.TargetReached()==1){
MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
wait(1);
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
einfahren_step = 4;
}
break;
case 4:
if (MyEpos5.TargetReached()==1){
einfahren = false;
current_step = 2;
wait(1);
MyEpos5.Power(0);
}
break;
}
}
led3 = 0;
break;
}//case
}//while
}//main