mmotor / Mbed 2 deprecated 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@4:8fb30d6c2855, 2016-09-22 (annotated)

Committer:
ODEM
Date:
Thu Sep 22 07:58:15 2016 +0000
Revision:
4:8fb30d6c2855
Parent:
3:fbc9de097e4f
Child:
5:524e83ec5174
v.2.1 Fehler behoben

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ODEM 0:aaf583a75b97 1 #include "mbed.h"
ODEM 0:aaf583a75b97 2 #include "EPOS2.h"
joe_feubli 2:0f4e77a22fd9 3
ODEM 0:aaf583a75b97 4
ODEM 0:aaf583a75b97 5 //***********************************************************************************************************************************//
ODEM 0:aaf583a75b97 6 //*** Global_Var *******************************************************************************************************************//
ODEM 0:aaf583a75b97 7 //***********************************************************************************************************************************//
ODEM 0:aaf583a75b97 8
ODEM 0:aaf583a75b97 9 Serial pc(USBTX, USBRX); // (tx, rx)
ODEM 0:aaf583a75b97 10 CAN can(p9, p10); // Can Pin def
ODEM 0:aaf583a75b97 11
joe_feubli 2:0f4e77a22fd9 12 /*
ODEM 0:aaf583a75b97 13 int velocity = 3000;
ODEM 0:aaf583a75b97 14 int acceleration = 50000;
ODEM 0:aaf583a75b97 15 int deceleration = 50000;
ODEM 0:aaf583a75b97 16
ODEM 0:aaf583a75b97 17 int anzahl_zyklen = 1000000;
ODEM 0:aaf583a75b97 18 int absolvierte_zyklen = 0;
ODEM 0:aaf583a75b97 19 int ActualPos = 0;
ODEM 0:aaf583a75b97 20 int DemandPos = 0;
ODEM 0:aaf583a75b97 21 int ActualCurrent = 0;
ODEM 0:aaf583a75b97 22 int DemandCurrent = 0;
ODEM 0:aaf583a75b97 23 int counter=0;
ODEM 1:c53aafa72b36 24 int counter2=0;
ODEM 0:aaf583a75b97 25 bool start = true;
ODEM 4:8fb30d6c2855 26
ODEM 1:c53aafa72b36 27 int counter_s = false;
ODEM 1:c53aafa72b36 28 int counter_s2 = false;
joe_feubli 3:fbc9de097e4f 29 */
joe_feubli 3:fbc9de097e4f 30 int ausfahren = false;
joe_feubli 3:fbc9de097e4f 31 int einfahren = false;
ODEM 4:8fb30d6c2855 32 int current_step = 1;
ODEM 4:8fb30d6c2855 33
joe_feubli 2:0f4e77a22fd9 34 int main(){ ///////// Main
ODEM 0:aaf583a75b97 35 pc.baud(9600);
ODEM 1:c53aafa72b36 36
ODEM 0:aaf583a75b97 37 wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
ODEM 0:aaf583a75b97 38
ODEM 0:aaf583a75b97 39 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 40
ODEM 0:aaf583a75b97 41 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 42
ODEM 0:aaf583a75b97 43 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 44 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 45
joe_feubli 2:0f4e77a22fd9 46 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 47 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 48 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 49 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 50 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 51 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 52 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 53
ODEM 0:aaf583a75b97 54 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 2:0f4e77a22fd9 55
joe_feubli 2:0f4e77a22fd9 56 //************** Initialisierung **************//
joe_feubli 2:0f4e77a22fd9 57
joe_feubli 2:0f4e77a22fd9 58 MyEpos1.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 59 MyEpos1.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 60 MyEpos1.Reset();
joe_feubli 2:0f4e77a22fd9 61
joe_feubli 2:0f4e77a22fd9 62 MyEpos2.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 63 MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 64 MyEpos2.Reset();
joe_feubli 2:0f4e77a22fd9 65
joe_feubli 2:0f4e77a22fd9 66 MyEpos3.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 67 MyEpos3.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 68 MyEpos3.Reset();
joe_feubli 2:0f4e77a22fd9 69
joe_feubli 2:0f4e77a22fd9 70 MyEpos4.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 71 MyEpos4.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 72 MyEpos4.Reset();
joe_feubli 2:0f4e77a22fd9 73
joe_feubli 2:0f4e77a22fd9 74 MyEpos5.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 75 MyEpos5.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 76 MyEpos5.Reset();
joe_feubli 2:0f4e77a22fd9 77
joe_feubli 2:0f4e77a22fd9 78 MyEpos6.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 79 MyEpos6.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 80 MyEpos6.Reset();
joe_feubli 2:0f4e77a22fd9 81
joe_feubli 2:0f4e77a22fd9 82 MyEpos7.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
joe_feubli 2:0f4e77a22fd9 83 MyEpos7.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 84 MyEpos7.Reset();
ODEM 0:aaf583a75b97 85
ODEM 0:aaf583a75b97 86 while(1){
joe_feubli 2:0f4e77a22fd9 87 switch (current_step){
joe_feubli 2:0f4e77a22fd9 88 case 1://HOMING:
joe_feubli 3:fbc9de097e4f 89 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 3:fbc9de097e4f 90 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 2:0f4e77a22fd9 91 MyEpos7.Homing(); //Bohrer
ODEM 4:8fb30d6c2855 92 while (MyEpos2.GetDigIn(4)!=1 or MyEpos4.GetDigIn(4)!=1){ //warten solange Solarpanels Aussen NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 93 wait(0.1);
joe_feubli 2:0f4e77a22fd9 94 }
joe_feubli 3:fbc9de097e4f 95 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 3:fbc9de097e4f 96 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 2:0f4e77a22fd9 97
ODEM 4:8fb30d6c2855 98 while (MyEpos7.GetDigIn(4)!=1){ //warten solange Bohrer NICHT eingefahren
joe_feubli 2:0f4e77a22fd9 99 wait(0.1);
joe_feubli 2:0f4e77a22fd9 100 }
joe_feubli 2:0f4e77a22fd9 101 MyEpos6.Homing();//Bohrgestell
ODEM 4:8fb30d6c2855 102 while (MyEpos1.GetDigIn(4)!=1 or MyEpos3.GetDigIn(4)!=1) { //warten solange Solarpanels Innen NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 103 wait(0.1);
joe_feubli 2:0f4e77a22fd9 104 }
joe_feubli 2:0f4e77a22fd9 105 MyEpos5.Homing();//Kopf
ODEM 4:8fb30d6c2855 106 while (MyEpos5.GetDigIn(4)!=1) { //warten solange Kopf NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 107 wait(0.1);
joe_feubli 2:0f4e77a22fd9 108 }
joe_feubli 2:0f4e77a22fd9 109 current_step = 2;//BEREIT;
joe_feubli 2:0f4e77a22fd9 110 break;
joe_feubli 2:0f4e77a22fd9 111
joe_feubli 2:0f4e77a22fd9 112
joe_feubli 2:0f4e77a22fd9 113 case 2://BEREIT:
ODEM 4:8fb30d6c2855 114 while (MyEpos6.GetDigIn(1)!=1) { //Kontrolle ob: Kopf in Grundposition
joe_feubli 2:0f4e77a22fd9 115 wait_ms(10);
joe_feubli 2:0f4e77a22fd9 116 }
joe_feubli 3:fbc9de097e4f 117 current_step = 3;//START_AUSFAHREN
joe_feubli 2:0f4e77a22fd9 118 break;
ODEM 1:c53aafa72b36 119
joe_feubli 2:0f4e77a22fd9 120
joe_feubli 3:fbc9de097e4f 121 case 3://START_AUSFAHREN:
joe_feubli 3:fbc9de097e4f 122 while(ausfahren == true){
joe_feubli 3:fbc9de097e4f 123 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 124 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 125 MyEpos2.MoveAbsolute(500,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 126 MyEpos4.MoveAbsolute(500,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 127 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 128 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 129 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 130 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 131 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 132 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 133 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 134 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 135 ausfahren = false;
joe_feubli 3:fbc9de097e4f 136 einfahren = true;
joe_feubli 3:fbc9de097e4f 137 current_step = 4;//START_EINFAHREN
joe_feubli 3:fbc9de097e4f 138 }
joe_feubli 3:fbc9de097e4f 139 }
joe_feubli 3:fbc9de097e4f 140 }
joe_feubli 3:fbc9de097e4f 141 }
joe_feubli 3:fbc9de097e4f 142 }
joe_feubli 3:fbc9de097e4f 143 }
joe_feubli 3:fbc9de097e4f 144
joe_feubli 2:0f4e77a22fd9 145 break;
ODEM 1:c53aafa72b36 146
joe_feubli 3:fbc9de097e4f 147 case 4://Start_EINFAHREN
joe_feubli 3:fbc9de097e4f 148 while(einfahren == true){
joe_feubli 3:fbc9de097e4f 149 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 150 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 151 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 152 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 153 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 154 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 155 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 156 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 157 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 158 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 159 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 160 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 161 einfahren = false;
joe_feubli 3:fbc9de097e4f 162 current_step = 2;//Bereit
joe_feubli 3:fbc9de097e4f 163 }
joe_feubli 3:fbc9de097e4f 164 }
joe_feubli 3:fbc9de097e4f 165 }
joe_feubli 3:fbc9de097e4f 166 }
joe_feubli 3:fbc9de097e4f 167 }
joe_feubli 3:fbc9de097e4f 168 }
joe_feubli 3:fbc9de097e4f 169
joe_feubli 3:fbc9de097e4f 170 break;
joe_feubli 3:fbc9de097e4f 171
joe_feubli 3:fbc9de097e4f 172
joe_feubli 3:fbc9de097e4f 173 case 5://USV:
ODEM 4:8fb30d6c2855 174 while (MyEpos1.GetDigIn()){
joe_feubli 3:fbc9de097e4f 175 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 176 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 177 MyEpos7.MoveAbsolute(0,200,500,500); // Bohrer
joe_feubli 3:fbc9de097e4f 178 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 179 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 180 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 181 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 182 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
ODEM 4:8fb30d6c2855 183 }
joe_feubli 3:fbc9de097e4f 184 }
joe_feubli 3:fbc9de097e4f 185 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 186 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 187 }
joe_feubli 3:fbc9de097e4f 188 }
joe_feubli 2:0f4e77a22fd9 189 break;
ODEM 1:c53aafa72b36 190
joe_feubli 3:fbc9de097e4f 191 case 6://ERROR2
ODEM 1:c53aafa72b36 192 break;
joe_feubli 2:0f4e77a22fd9 193 }//case
joe_feubli 2:0f4e77a22fd9 194 }//while
joe_feubli 2:0f4e77a22fd9 195 }//main