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@3:fbc9de097e4f, 2016-09-22 (annotated)

Committer:
joe_feubli
Date:
Thu Sep 22 07:39:21 2016 +0000
Revision:
3:fbc9de097e4f
Parent:
2:0f4e77a22fd9
Child:
4:8fb30d6c2855
V2.0 (Programm ohne USV-Interrupt) noch nicht getestet

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;
joe_feubli 2:0f4e77a22fd9 26 int current_step = 1;
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;
joe_feubli 2:0f4e77a22fd9 32
joe_feubli 2:0f4e77a22fd9 33 int main(){ ///////// Main
ODEM 0:aaf583a75b97 34 pc.baud(9600);
ODEM 1:c53aafa72b36 35
ODEM 0:aaf583a75b97 36 wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
ODEM 0:aaf583a75b97 37
ODEM 0:aaf583a75b97 38 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 39
ODEM 0:aaf583a75b97 40 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 41
ODEM 0:aaf583a75b97 42 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 43 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 44
joe_feubli 2:0f4e77a22fd9 45 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 46 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 47 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 48 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 49 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 50 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 51 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 52
ODEM 0:aaf583a75b97 53 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 2:0f4e77a22fd9 54
joe_feubli 2:0f4e77a22fd9 55 //************** Initialisierung **************//
joe_feubli 2:0f4e77a22fd9 56
joe_feubli 2:0f4e77a22fd9 57 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 58 MyEpos1.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 59 MyEpos1.Reset();
joe_feubli 2:0f4e77a22fd9 60
joe_feubli 2:0f4e77a22fd9 61 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 62 MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 63 MyEpos2.Reset();
joe_feubli 2:0f4e77a22fd9 64
joe_feubli 2:0f4e77a22fd9 65 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 66 MyEpos3.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 67 MyEpos3.Reset();
joe_feubli 2:0f4e77a22fd9 68
joe_feubli 2:0f4e77a22fd9 69 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 70 MyEpos4.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 71 MyEpos4.Reset();
joe_feubli 2:0f4e77a22fd9 72
joe_feubli 2:0f4e77a22fd9 73 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 74 MyEpos5.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 75 MyEpos5.Reset();
joe_feubli 2:0f4e77a22fd9 76
joe_feubli 2:0f4e77a22fd9 77 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 78 MyEpos6.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 79 MyEpos6.Reset();
joe_feubli 2:0f4e77a22fd9 80
joe_feubli 2:0f4e77a22fd9 81 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 82 MyEpos7.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 83 MyEpos7.Reset();
ODEM 0:aaf583a75b97 84
ODEM 0:aaf583a75b97 85 while(1){
joe_feubli 2:0f4e77a22fd9 86 switch (current_step){
joe_feubli 2:0f4e77a22fd9 87 case 1://HOMING:
joe_feubli 3:fbc9de097e4f 88 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 3:fbc9de097e4f 89 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 2:0f4e77a22fd9 90 MyEpos7.Homing(); //Bohrer
joe_feubli 3:fbc9de097e4f 91 while (MyEpos2.GetDigIn(4)!1) or (MyEpos4.GetDigIn(4)!1){ //warten solange Solarpanels Aussen NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 92 wait(0.1);
joe_feubli 2:0f4e77a22fd9 93 }
joe_feubli 3:fbc9de097e4f 94 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 3:fbc9de097e4f 95 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 2:0f4e77a22fd9 96
joe_feubli 3:fbc9de097e4f 97 while (MyEpos7.GetDigIn(4)!1){ //warten solange Bohrer NICHT eingefahren
joe_feubli 2:0f4e77a22fd9 98 wait(0.1);
joe_feubli 2:0f4e77a22fd9 99 }
joe_feubli 2:0f4e77a22fd9 100 MyEpos6.Homing();//Bohrgestell
joe_feubli 3:fbc9de097e4f 101 while (MyEpos1.GetDigIn(4)!1) or (MyEpos3.GetDigIn(4)!1) { //warten solange Solarpanels Innen NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 102 wait(0.1);
joe_feubli 2:0f4e77a22fd9 103 }
joe_feubli 2:0f4e77a22fd9 104 MyEpos5.Homing();//Kopf
joe_feubli 3:fbc9de097e4f 105 while (MyEpos5.GetDigIn(4)!1) { //warten solange Kopf NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 106 wait(0.1);
joe_feubli 2:0f4e77a22fd9 107 }
joe_feubli 2:0f4e77a22fd9 108 current_step = 2;//BEREIT;
joe_feubli 2:0f4e77a22fd9 109 break;
joe_feubli 2:0f4e77a22fd9 110
joe_feubli 2:0f4e77a22fd9 111
joe_feubli 2:0f4e77a22fd9 112 case 2://BEREIT:
joe_feubli 2:0f4e77a22fd9 113 while (MyEpos6.GetDigIn(1)!1) { //Kontrolle ob: Kopf in Grundposition
joe_feubli 2:0f4e77a22fd9 114 wait_ms(10);
joe_feubli 2:0f4e77a22fd9 115 }
joe_feubli 3:fbc9de097e4f 116 current_step = 3;//START_AUSFAHREN
joe_feubli 2:0f4e77a22fd9 117 break;
ODEM 1:c53aafa72b36 118
joe_feubli 2:0f4e77a22fd9 119
joe_feubli 3:fbc9de097e4f 120 case 3://START_AUSFAHREN:
joe_feubli 3:fbc9de097e4f 121 while(ausfahren == true){
joe_feubli 3:fbc9de097e4f 122 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 123 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 124 MyEpos2.MoveAbsolute(500,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 125 MyEpos4.MoveAbsolute(500,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 126 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 127 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 128 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 129 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 130 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 131 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 132 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 133 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 134 ausfahren = false;
joe_feubli 3:fbc9de097e4f 135 einfahren = true;
joe_feubli 3:fbc9de097e4f 136 current_step = 4;//START_EINFAHREN
joe_feubli 3:fbc9de097e4f 137 }
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 2:0f4e77a22fd9 144 break;
ODEM 1:c53aafa72b36 145
joe_feubli 3:fbc9de097e4f 146 case 4://Start_EINFAHREN
joe_feubli 3:fbc9de097e4f 147 while(einfahren == true){
joe_feubli 3:fbc9de097e4f 148 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 149 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 150 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 151 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 152 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 153 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 154 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 155 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 156 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 157 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 158 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 159 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 160 einfahren = false;
joe_feubli 3:fbc9de097e4f 161 current_step = 2;//Bereit
joe_feubli 3:fbc9de097e4f 162 }
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 break;
joe_feubli 3:fbc9de097e4f 170
joe_feubli 3:fbc9de097e4f 171
joe_feubli 3:fbc9de097e4f 172 case 5://USV:
joe_feubli 3:fbc9de097e4f 173 while (MyEpos.GetDigIn()){
joe_feubli 3:fbc9de097e4f 174 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 175 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 176 MyEpos7.MoveAbsolute(0,200,500,500); // Bohrer
joe_feubli 3:fbc9de097e4f 177 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 178 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 179 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 180 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 181 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 182 }
joe_feubli 3:fbc9de097e4f 183 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 184 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 185 }
joe_feubli 3:fbc9de097e4f 186 }
joe_feubli 2:0f4e77a22fd9 187 break;
ODEM 1:c53aafa72b36 188
joe_feubli 3:fbc9de097e4f 189 case 6://ERROR2
ODEM 1:c53aafa72b36 190 break;
joe_feubli 2:0f4e77a22fd9 191 }//case
joe_feubli 2:0f4e77a22fd9 192 }//while
joe_feubli 2:0f4e77a22fd9 193 }//main