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-11-03
- Revision:
- 14:bee23de5dff0
- Parent:
- 13:cc43e546f310
- Child:
- 15:c31c862632c0
- Child:
- 16:e5343d6c9fb2
File content as of revision 14:bee23de5dff0:
#include "mbed.h"
#include "EPOS2.h"
//***********************************************************************************************************************************//
//*** Global_Var *******************************************************************************************************************//
//***********************************************************************************************************************************//
Serial pc(USBTX, USBRX); // (tx, rx)
CAN can(p9, p10); // Can Pin def
DigitalIn usv_ready(p23);
InterruptIn buffering(p24);
DigitalIn replace_battery(p25);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
bool epos1_homing = false;
bool epos3_homing = false;
bool node1_homed = false;
bool node2_homed = false;
bool node3_homed = false;
bool node4_homed = false;
bool node5_homed = false;
bool node6_homed = false;
bool node7_homed = false;
bool Power_ein = true;
bool node5_grundposition = false;
bool ausfahren = false;
bool einfahren = false;
bool usv = false;
int current_step = 1; //HOMING
int ausfahren_step = 1;
int einfahren_step = 1;
int usv_step = 1;
int ErrorState1;
int ErrorState2;
int ErrorState3;
int ErrorState4;
int ErrorState5;
int ErrorState6;
int ErrorState7;
float speed = 1.5;
void interrupt_usv(){ //Funktion, wird aufgeruffen durch den interrupt des USV's
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 24/5 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");
wait(0.1);
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);
while(1){
if(MyEpos1.GetError() != 0 or MyEpos2.GetError() != 0 or MyEpos3.GetError() != 0 or MyEpos4.GetError() != 0 or MyEpos5.GetError() != 0 /*or MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0*/){
current_step = 7; //FEHLER
MyEpos1.MoveVelocity(0, 500, 500);
MyEpos2.MoveVelocity(0, 500, 500);
MyEpos3.MoveVelocity(0, 500, 500);
MyEpos4.MoveVelocity(0, 500, 500);
MyEpos5.MoveVelocity(0, 500, 500);
wait(0.1);
ErrorState1 = MyEpos1.GetError();
ErrorState2 = MyEpos2.GetError();
ErrorState3 = MyEpos3.GetError();
ErrorState4 = MyEpos4.GetError();
ErrorState5 = MyEpos5.GetError();
wait(0.1);
pc.printf("ErrorState Panels/Kopf: Panel Aussen Linkss=%d, Panel Innen Links=%d, Panel Aussen Rechts=%d, Panel Innen Rechts=%d, Kopf=%d\r\n", ErrorState1, ErrorState2, ErrorState3, ErrorState4, ErrorState5);
}
if(MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0){
current_step = 6; //FEHLER
MyEpos1.MoveVelocity(0, 500, 500);
MyEpos2.MoveVelocity(0, 500, 500);
MyEpos3.MoveVelocity(0, 500, 500);
wait(0.1);
ErrorState6 = MyEpos6.GetError();
ErrorState7 = MyEpos7.GetError();
wait(0.1);
pc.printf("Bohrgestell/Bohrer ErrorState: Bohrgestell=%d, Bohrer=%d\r\n", ErrorState6, ErrorState7);
}
switch (current_step){
case 1://HOMING:
led1 = 1;
if (Power_ein == true){
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
MyEpos6.Power(1);
MyEpos7.Power(1);
Power_ein = false;
}
/***********************HOMING*************************/
if (node1_homed == false){
if(epos1_homing == false){
MyEpos1.MoveVelocity(-400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
epos1_homing = true;
}
if(MyEpos1.GetDigIn(4) == 1){
pc.printf("Homing1\n");
MyEpos1.Homing(); //Solarpanel_Aussen_Links
node1_homed = true;
break;
}
break;
}
if (node3_homed == false){
if (epos3_homing == false){
MyEpos3.MoveVelocity(400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
epos3_homing = true;
}
if(MyEpos3.GetDigIn(4) == 1){
pc.printf("Homing3\n");
MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
node3_homed = true;
}
break;
}
if (node5_homed == false){
pc.printf("Homing5\n");
MyEpos5.Homing(); //Kopf
MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
pc.printf("Fahren auf -60000\n");
while(MyEpos5.TargetReached()!=1){
if(MyEpos5.GetError() == 1){
break;
}
}
pc.printf("Target reached\n");
node5_homed = true;
break;
}
if (node7_homed == false){
pc.printf("Homing7\n");
MyEpos7.Homing(); //Bohrer
node7_homed = true;
break;
}
if (node1_homed == true and node2_homed == false){
if (node5_grundposition == false){
MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
}
pc.printf("Homing2\n");
MyEpos2.Homing(); //Solarpanel_Innen_Links
node2_homed = true;
break;
}
if (node3_homed == true and node4_homed == false){
pc.printf("Homing4\n");
MyEpos4.Homing(); //Solarpanel_Innen_Rechts
node4_homed = true;
MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
wait(0.1);
break;
}
if (node7_homed == true and node6_homed == false){
pc.printf("Homing6\n");
MyEpos6.Homing();//Bohrgestell
node6_homed = true;
break;
}
if (node2_homed == true and node4_homed == true and node5_grundposition == false){
//MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
wait(0.1);
while(MyEpos5.TargetReached() != 1){
if(MyEpos5.GetError() == 1){
break;
}
wait(0.1);
}
node5_grundposition = 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){
pc.printf("Homing beendet\n");
node5_grundposition = false;
Power_ein = true;
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
wait(0.1);
MyEpos5.Power(0);
current_step = 2;//BEREIT;
}
led1 = 0;
break;
case 2://BEREIT:
led2 = 1;
MyEpos5.SetDigOut(4,0);
if (MyEpos5.GetDigIn(3) == 1){
MyEpos5.SetDigOut(4,1);
wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
MyEpos6.Power(1);
MyEpos7.Power(1);
einfahren = false;
ausfahren_step = 1;
current_step = 3;
wait(0.5);
}
wait(0.1);
/********TEST********/ /*
MyEpos5.SetDigOut(4,1);
wait_ms(10);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
MyEpos6.Power(1);
MyEpos7.Power(1);
einfahren = false;
ausfahren_step = 1;
current_step = 3;
*/ /********TEST*******/
led2 = 0;
break;
case 3://ABLAUF
// pc.printf("Läuft...\n");
led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
case 1:
MyEpos5.MoveAbsolute(-150000,800*speed,100*speed,100*speed); //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*speed,100*speed,100*speed); //Panel Innen Links
MyEpos4.MoveAbsolute(-310000,500*speed,100*speed,100*speed); //Panel Innen Rechts
ausfahren_step = 3;
wait(1);
}
wait(0.1);
break;
case 3:
if (MyEpos2.GetActualPos() > 280000 and MyEpos4.GetActualPos() < -280000){
MyEpos1.MoveAbsolute(318000,500*speed,100*speed,100*speed); //Panel Aussen Links
MyEpos3.MoveAbsolute(-318000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
ausfahren_step = 4;
wait(1);
}
wait(0.1);
break;
case 4:
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
MyEpos6.MoveAbsolute(-76000,500*speed,100*speed,100*speed); //Bohrgestell
ausfahren_step = 5;
wait(1);
}
wait(0.1);
break;
case 5:
if (MyEpos6.TargetReached()==1){
MyEpos7.MoveAbsolute(700000,2000*speed,500*speed,500*speed); //Bohrer
ausfahren_step = 6;
wait(1);
}
wait(0.1);
break;
case 6:
if (MyEpos7.TargetReached()==1){
einfahren = true;
einfahren_step = 1;
wait(1);
}
wait(0.1);
// while(buffering == 0){//für Fotograf
// }
break;
/*
case 4:
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
einfahren = true;
einfahren_step = 3;
wait(1);
}
wait(0.1);
break;
*/
}//switch (ausfahren_step)
}//if (einfahren == false)
if (einfahren == true){
ausfahren_step = 1;
switch (einfahren_step){
case 1:
MyEpos7.MoveAbsolute(0,2000*speed,500*speed,500*speed); //Bohrer
einfahren_step = 2;
wait(0.1);
break;
case 2:
if (MyEpos7.TargetReached()==1){
MyEpos6.MoveAbsolute(0,500*speed,100*speed,100*speed); //Bohrgestell
einfahren_step = 3;
wait(1);
}
wait(0.1);
break;
case 3:
// if (MyEpos7.TargetReached()==1){
MyEpos1.MoveAbsolute(10000,500*speed,100*speed,100*speed); //Panel Aussen Links
MyEpos3.MoveAbsolute(-10000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
einfahren_step = 4;
// wait(1);
// }
wait(0.1);
break;
case 4:
if (MyEpos1.GetActualPos() < 20000 and MyEpos3.GetActualPos() > -20000){
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
MyEpos2.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Links
MyEpos4.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Rechts
einfahren_step = 5;
wait(0.1);
}
wait(0.1);
break;
case 5:
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
MyEpos5.MoveAbsolute(1000,500*speed,100*speed,100*speed); //Kopf
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
einfahren_step = 6;
wait(0.1);
}
wait(0.1);
break;
case 6:
if (MyEpos5.TargetReached()==1){
einfahren = false;
current_step = 2;
wait(1);
MyEpos5.Power(0);
}
wait(0.1);
break;
}//switch(einfahren_step)
}//if einfahren == true)
led3 = 0;
break;
case 5://USV:
pc.printf("USV\n");
led4 = 1;
MyEpos5.SetDigOut(4,1);
switch (usv_step){
case 0:
pc.printf("USV 0\n");
wait(0.1);
MyEpos1.Power(1);
MyEpos2.Power(1);
MyEpos3.Power(1);
MyEpos4.Power(1);
MyEpos5.Power(1);
MyEpos6.Power(1);
MyEpos7.Power(1);
usv_step = 1;
wait(0.1);
break;
case 1:
pc.printf("USV 1\n");
MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
MyEpos7.MoveAbsolute(0,2000,500,500); //Bohrer
wait(0.1);
if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1 and MyEpos7.TargetReached()==1){
usv_step = 2;
wait(0.1);
}
break;
case 2:
pc.printf("USV 2\n");
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
MyEpos6.MoveAbsolute(0,500,100,100); //Bohrgestell
wait(0.1);
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
usv_step = 3;
wait(0.1);
}
break;
case 3:
pc.printf("USV 3\n");
MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
wait(0.1);
if (MyEpos5.TargetReached()==1){
usv_step = 4;
wait(0.1);
}
break;
case 4:
pc.printf("USV 4\n");
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
MyEpos4.Power(0);
MyEpos5.Power(0);
MyEpos6.Power(0);
MyEpos7.Power(0);
usv_step = 5;
wait(0.1);
break;
case 5:
pc.printf("USV 5\n");
if(buffering == 1){
led3 = 1;
} else {
led3 = 0;
Power_ein = true;
current_step=1;
usv_step=0;
ausfahren_step=1;
einfahren_step=1;
wait(0.1);
break;
}
wait(1);
break;
}//switch(usv_step)
led4 = 0;
break;
case 6: //Fehler Bohrer/Bohrgestell
pc.printf("Fehler RESET Bohrer\n");
MyEpos7.MoveVelocity(0, 500, 500);
if (MyEpos7.TargetReached() == 1){
MyEpos6.MoveVelocity(0, 500, 500);
if(MyEpos6.TargetReached() == 1){
MyEpos5.Reset();
MyEpos5.Power(1);
MyEpos6.Reset();
MyEpos6.Power(1);
current_step = 1;
}
}
//node1_homed = false;
//node2_homed = false;
//current_step = 1;
break;
case 7:
pc.printf("Fehler RESET\n");
MyEpos1.MoveVelocity(0, 500, 500);
MyEpos2.MoveVelocity(0, 500, 500);
MyEpos3.MoveVelocity(0, 500, 500);
MyEpos4.MoveVelocity(0, 500, 500);
MyEpos5.MoveVelocity(0, 500, 500);
wait(1);
MyEpos1.Reset();
MyEpos1.Power(1);
MyEpos2.Reset();
MyEpos2.Power(1);
MyEpos3.Reset();
MyEpos3.Power(1);
MyEpos4.Reset();
MyEpos4.Power(1);
MyEpos5.Reset();
MyEpos5.Power(1);
epos1_homing = false;
epos3_homing = false;
node1_homed = false;
node2_homed = false;
node3_homed = false;
node4_homed = false;
node5_homed = false;
Power_ein = true;
if(MyEpos2.GetActualPos() < 100000 or MyEpos4.GetActualPos() > -100000){
MyEpos1.Homing();
MyEpos3.Homing();
MyEpos1.MoveAbsolute(0,500,100,100);
MyEpos3.MoveAbsolute(0,500,100,100);
MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
while (MyEpos2.TargetReached() != 1 or MyEpos4.TargetReached() != 1){
if(MyEpos2.GetError() == 1 or MyEpos4.GetError() == 1){
//wait(0.1);
break;
}
//wait(0.1);
}
}
current_step = 6;
wait(0.1);
break;
}//switch (current_step)
}//while
}//main