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@5:524e83ec5174, 2016-09-29 (annotated)

Committer:
joe_feubli
Date:
Thu Sep 29 10:12:24 2016 +0000
Revision:
5:524e83ec5174
Parent:
4:8fb30d6c2855
Child:
6:3eaa420dc2f5
Erste Tests mit Homing

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 5:524e83ec5174 30 DigitalIn mySwitch1(p21);
joe_feubli 5:524e83ec5174 31 DigitalIn mySwitch2(p22);
joe_feubli 5:524e83ec5174 32 DigitalIn mySwitch3(p23);
joe_feubli 5:524e83ec5174 33 /*mySwitch1.mode(PullUp);
joe_feubli 5:524e83ec5174 34 mySwitch2.mode(PullUp);
joe_feubli 5:524e83ec5174 35 mySwitch3.mode(PullUp);*/
joe_feubli 3:fbc9de097e4f 36 int ausfahren = false;
joe_feubli 3:fbc9de097e4f 37 int einfahren = false;
ODEM 4:8fb30d6c2855 38 int current_step = 1;
ODEM 4:8fb30d6c2855 39
joe_feubli 2:0f4e77a22fd9 40 int main(){ ///////// Main
ODEM 0:aaf583a75b97 41 pc.baud(9600);
ODEM 1:c53aafa72b36 42
ODEM 0:aaf583a75b97 43 wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
ODEM 0:aaf583a75b97 44
ODEM 0:aaf583a75b97 45 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 46
ODEM 0:aaf583a75b97 47 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 48
ODEM 0:aaf583a75b97 49 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 50 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 51
joe_feubli 2:0f4e77a22fd9 52 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 53 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 54 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 55 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 56 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 57 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 58 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 59
ODEM 0:aaf583a75b97 60 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 2:0f4e77a22fd9 61
joe_feubli 2:0f4e77a22fd9 62 //************** Initialisierung **************//
joe_feubli 2:0f4e77a22fd9 63
joe_feubli 5:524e83ec5174 64 /* HOMING METHODEN:
joe_feubli 5:524e83ec5174 65 Homing Method 7 :Home Switch Positive Speed & Index
joe_feubli 5:524e83ec5174 66 Homing Method 11:Home Switch Negative Speed & Index
joe_feubli 5:524e83ec5174 67 Homing Method 23: Home Switch Positive Speed
joe_feubli 5:524e83ec5174 68 Homing Method 27: Home Switch Negative Speed
joe_feubli 5:524e83ec5174 69 */
joe_feubli 5:524e83ec5174 70 //Motortype 10: EC-Motor
joe_feubli 5:524e83ec5174 71
joe_feubli 5:524e83ec5174 72 MyEpos1.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 73 MyEpos1.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 74 MyEpos1.Reset();
joe_feubli 2:0f4e77a22fd9 75
joe_feubli 5:524e83ec5174 76 MyEpos2.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 77 MyEpos2.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 78 MyEpos2.Reset();
joe_feubli 2:0f4e77a22fd9 79
joe_feubli 5:524e83ec5174 80 MyEpos3.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 81 MyEpos3.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 2:0f4e77a22fd9 82 MyEpos3.Reset();
joe_feubli 2:0f4e77a22fd9 83
joe_feubli 5:524e83ec5174 84 MyEpos4.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 85 MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 86 MyEpos4.Reset();
joe_feubli 2:0f4e77a22fd9 87
joe_feubli 5:524e83ec5174 88 MyEpos5.SetPar(10,4430, 7, 3500, 40); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 89 MyEpos5.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 90 MyEpos5.Reset();
joe_feubli 2:0f4e77a22fd9 91
joe_feubli 5:524e83ec5174 92 MyEpos6.SetPar(10,3440, 1, 12000, 3.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 93 MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 94 MyEpos6.Reset();
joe_feubli 2:0f4e77a22fd9 95
joe_feubli 2:0f4e77a22fd9 96 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 5:524e83ec5174 97 MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 98 MyEpos7.Reset();
ODEM 0:aaf583a75b97 99
ODEM 0:aaf583a75b97 100 while(1){
joe_feubli 2:0f4e77a22fd9 101 switch (current_step){
joe_feubli 2:0f4e77a22fd9 102 case 1://HOMING:
joe_feubli 5:524e83ec5174 103
joe_feubli 5:524e83ec5174 104 MyEpos1.Reset();
joe_feubli 5:524e83ec5174 105 MyEpos2.Reset();
joe_feubli 5:524e83ec5174 106 MyEpos3.Reset();
joe_feubli 5:524e83ec5174 107 MyEpos4.Reset();
joe_feubli 5:524e83ec5174 108 MyEpos5.Reset();
joe_feubli 5:524e83ec5174 109 MyEpos6.Reset();
joe_feubli 5:524e83ec5174 110 MyEpos7.Reset();
joe_feubli 5:524e83ec5174 111
joe_feubli 5:524e83ec5174 112 // MyEpos1.Power(1);
joe_feubli 5:524e83ec5174 113 // MyEpos2.Power(1);
joe_feubli 5:524e83ec5174 114 MyEpos3.Power(1);
joe_feubli 5:524e83ec5174 115 MyEpos4.Power(1);
joe_feubli 5:524e83ec5174 116 MyEpos5.Power(1);
joe_feubli 5:524e83ec5174 117 MyEpos6.Power(1);
joe_feubli 5:524e83ec5174 118 // MyEpos7.Power(1);
joe_feubli 5:524e83ec5174 119 // MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 5:524e83ec5174 120 // MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 5:524e83ec5174 121 // MyEpos7.Homing(); //Bohrer
joe_feubli 5:524e83ec5174 122 // while (/*MyEpos1.GetDigIn(4)!=1 or*/ MyEpos3.GetDigIn(4)!=1){ //warten solange Solarpanels Aussen NICHT in Grundposition
joe_feubli 5:524e83ec5174 123 // wait(0.1);
joe_feubli 5:524e83ec5174 124 // }
joe_feubli 5:524e83ec5174 125 // MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 3:fbc9de097e4f 126 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 2:0f4e77a22fd9 127
joe_feubli 5:524e83ec5174 128 /* while (MyEpos7.GetDigIn(4)!=1){ //warten solange Bohrer NICHT eingefahren
joe_feubli 2:0f4e77a22fd9 129 wait(0.1);
joe_feubli 2:0f4e77a22fd9 130 }
joe_feubli 2:0f4e77a22fd9 131 MyEpos6.Homing();//Bohrgestell
joe_feubli 5:524e83ec5174 132 */ while (/*MyEpos2.GetDigIn(4)!=1 or */MyEpos4.GetDigIn(4)!=1) { //warten solange Solarpanels Innen NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 133 wait(0.1);
joe_feubli 2:0f4e77a22fd9 134 }
joe_feubli 2:0f4e77a22fd9 135 MyEpos5.Homing();//Kopf
ODEM 4:8fb30d6c2855 136 while (MyEpos5.GetDigIn(4)!=1) { //warten solange Kopf NICHT in Grundposition
joe_feubli 2:0f4e77a22fd9 137 wait(0.1);
joe_feubli 2:0f4e77a22fd9 138 }
joe_feubli 2:0f4e77a22fd9 139 current_step = 2;//BEREIT;
joe_feubli 2:0f4e77a22fd9 140 break;
joe_feubli 2:0f4e77a22fd9 141
joe_feubli 2:0f4e77a22fd9 142
joe_feubli 2:0f4e77a22fd9 143 case 2://BEREIT:
joe_feubli 5:524e83ec5174 144 /* while (MyEpos6.GetDigIn(1)!=1) { //warten solange Buzzer nicht gedrückt
joe_feubli 2:0f4e77a22fd9 145 wait_ms(10);
joe_feubli 2:0f4e77a22fd9 146 }
joe_feubli 3:fbc9de097e4f 147 current_step = 3;//START_AUSFAHREN
joe_feubli 5:524e83ec5174 148 */ current_step = 6; //Test
joe_feubli 2:0f4e77a22fd9 149 break;
ODEM 1:c53aafa72b36 150
joe_feubli 2:0f4e77a22fd9 151
joe_feubli 3:fbc9de097e4f 152 case 3://START_AUSFAHREN:
joe_feubli 3:fbc9de097e4f 153 while(ausfahren == true){
joe_feubli 3:fbc9de097e4f 154 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 155 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 156 MyEpos2.MoveAbsolute(500,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 157 MyEpos4.MoveAbsolute(500,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 158 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 159 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 160 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 161 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 162 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 163 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 164 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 165 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 166 ausfahren = false;
joe_feubli 3:fbc9de097e4f 167 einfahren = true;
joe_feubli 3:fbc9de097e4f 168 current_step = 4;//START_EINFAHREN
joe_feubli 3:fbc9de097e4f 169 }
joe_feubli 3:fbc9de097e4f 170 }
joe_feubli 3:fbc9de097e4f 171 }
joe_feubli 3:fbc9de097e4f 172 }
joe_feubli 3:fbc9de097e4f 173 }
joe_feubli 3:fbc9de097e4f 174 }
joe_feubli 3:fbc9de097e4f 175
joe_feubli 2:0f4e77a22fd9 176 break;
ODEM 1:c53aafa72b36 177
joe_feubli 3:fbc9de097e4f 178 case 4://Start_EINFAHREN
joe_feubli 3:fbc9de097e4f 179 while(einfahren == true){
joe_feubli 3:fbc9de097e4f 180 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 181 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 182 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 183 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 184 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 185 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 186 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 187 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 188 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 189 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 190 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 191 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 192 einfahren = false;
joe_feubli 3:fbc9de097e4f 193 current_step = 2;//Bereit
joe_feubli 3:fbc9de097e4f 194 }
joe_feubli 3:fbc9de097e4f 195 }
joe_feubli 3:fbc9de097e4f 196 }
joe_feubli 3:fbc9de097e4f 197 }
joe_feubli 3:fbc9de097e4f 198 }
joe_feubli 3:fbc9de097e4f 199 }
joe_feubli 3:fbc9de097e4f 200
joe_feubli 3:fbc9de097e4f 201 break;
joe_feubli 3:fbc9de097e4f 202
joe_feubli 3:fbc9de097e4f 203
joe_feubli 3:fbc9de097e4f 204 case 5://USV:
joe_feubli 5:524e83ec5174 205 while (MyEpos1.GetDigIn(1)){
joe_feubli 3:fbc9de097e4f 206 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 207 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 208 MyEpos7.MoveAbsolute(0,200,500,500); // Bohrer
joe_feubli 3:fbc9de097e4f 209 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 210 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 211 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 212 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 213 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
ODEM 4:8fb30d6c2855 214 }
joe_feubli 3:fbc9de097e4f 215 }
joe_feubli 3:fbc9de097e4f 216 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 217 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 218 }
joe_feubli 3:fbc9de097e4f 219 }
joe_feubli 2:0f4e77a22fd9 220 break;
ODEM 1:c53aafa72b36 221
joe_feubli 5:524e83ec5174 222 case 6://Test
joe_feubli 5:524e83ec5174 223 MyEpos5.Power(1);
joe_feubli 5:524e83ec5174 224 wait(1);
joe_feubli 5:524e83ec5174 225 MyEpos5.MoveAbsolute(-151000,200,100,100); //Kopf
joe_feubli 5:524e83ec5174 226 wait_ms(10);
joe_feubli 5:524e83ec5174 227 while (MyEpos5.TargetReached()!=1){
joe_feubli 5:524e83ec5174 228 wait(1);
joe_feubli 5:524e83ec5174 229 }
joe_feubli 5:524e83ec5174 230 MyEpos5.MoveAbsolute(0,200,100,100); //Kopf
joe_feubli 5:524e83ec5174 231 wait_ms(10);
joe_feubli 5:524e83ec5174 232 while (MyEpos5.TargetReached()!=1){
joe_feubli 5:524e83ec5174 233 wait(1);
joe_feubli 5:524e83ec5174 234 }
joe_feubli 5:524e83ec5174 235 current_step = 2;//Bereit
ODEM 1:c53aafa72b36 236 break;
joe_feubli 2:0f4e77a22fd9 237 }//case
joe_feubli 2:0f4e77a22fd9 238 }//while
joe_feubli 2:0f4e77a22fd9 239 }//main