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@7:44f2f9fd2eeb, 2016-10-14 (annotated)

Committer:
joe_feubli
Date:
Fri Oct 14 08:33:11 2016 +0000
Revision:
7:44f2f9fd2eeb
Parent:
6:3eaa420dc2f5
Child:
8:98d4028cdea6
Ablauf mit USV (erste Funktionstests erfolgreich)

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 7:44f2f9fd2eeb 30 DigitalIn usv_ready(p23);
joe_feubli 7:44f2f9fd2eeb 31 InterruptIn buffering(p24);
joe_feubli 7:44f2f9fd2eeb 32 DigitalIn replace_battery(p25);
joe_feubli 6:3eaa420dc2f5 33
joe_feubli 6:3eaa420dc2f5 34 DigitalOut led1(LED1);
joe_feubli 6:3eaa420dc2f5 35 DigitalOut led2(LED2);
joe_feubli 6:3eaa420dc2f5 36 DigitalOut led3(LED3);
joe_feubli 6:3eaa420dc2f5 37 DigitalOut led4(LED4);
joe_feubli 6:3eaa420dc2f5 38
joe_feubli 6:3eaa420dc2f5 39 bool node1_homed = false;
joe_feubli 6:3eaa420dc2f5 40 bool node2_homed = false;
joe_feubli 6:3eaa420dc2f5 41 bool node3_homed = false;
joe_feubli 6:3eaa420dc2f5 42 bool node4_homed = true;
joe_feubli 6:3eaa420dc2f5 43 bool node5_homed = false;
joe_feubli 6:3eaa420dc2f5 44 bool node6_homed = true;
joe_feubli 6:3eaa420dc2f5 45 bool node7_homed = true;
joe_feubli 6:3eaa420dc2f5 46
joe_feubli 6:3eaa420dc2f5 47 int homing = false;
joe_feubli 6:3eaa420dc2f5 48 int ready = false;
joe_feubli 3:fbc9de097e4f 49 int ausfahren = false;
joe_feubli 6:3eaa420dc2f5 50 int einfahren = false;
joe_feubli 6:3eaa420dc2f5 51 int usv = false;
joe_feubli 6:3eaa420dc2f5 52 int current_step = 1; //HOMING
joe_feubli 6:3eaa420dc2f5 53 int ausfahren_step = 1;
joe_feubli 7:44f2f9fd2eeb 54 int einfahren_step = 1;
joe_feubli 7:44f2f9fd2eeb 55 int usv_step = 1;
joe_feubli 6:3eaa420dc2f5 56
joe_feubli 6:3eaa420dc2f5 57 void interrupt_usv(){
joe_feubli 6:3eaa420dc2f5 58 current_step = 5;
joe_feubli 7:44f2f9fd2eeb 59 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 60 }
ODEM 4:8fb30d6c2855 61
joe_feubli 2:0f4e77a22fd9 62 int main(){ ///////// Main
joe_feubli 7:44f2f9fd2eeb 63 led1 = 1;
ODEM 0:aaf583a75b97 64 pc.baud(9600);
ODEM 1:c53aafa72b36 65
joe_feubli 7:44f2f9fd2eeb 66 buffering.rise(&interrupt_usv);
joe_feubli 7:44f2f9fd2eeb 67
joe_feubli 7:44f2f9fd2eeb 68 wait(0.5); //Wartezeit bis Epos2 70/10 aufgestartet ist
joe_feubli 7:44f2f9fd2eeb 69 led2 = 1;
joe_feubli 7:44f2f9fd2eeb 70 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 71 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 72 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 73 led4 = 1;
joe_feubli 7:44f2f9fd2eeb 74 wait(0.5);
ODEM 0:aaf583a75b97 75
ODEM 0:aaf583a75b97 76 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 77
ODEM 0:aaf583a75b97 78 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 79
ODEM 0:aaf583a75b97 80 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 81 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 82
joe_feubli 2:0f4e77a22fd9 83 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 84 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 85 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 86 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 87 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 88 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 89 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 90
ODEM 0:aaf583a75b97 91 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 2:0f4e77a22fd9 92
joe_feubli 6:3eaa420dc2f5 93 MyEpos1.Reset();
joe_feubli 6:3eaa420dc2f5 94 MyEpos2.Reset();
joe_feubli 6:3eaa420dc2f5 95 MyEpos3.Reset();
joe_feubli 6:3eaa420dc2f5 96 MyEpos4.Reset();
joe_feubli 6:3eaa420dc2f5 97 MyEpos5.Reset();
joe_feubli 6:3eaa420dc2f5 98 MyEpos6.Reset();
joe_feubli 6:3eaa420dc2f5 99 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 100 wait(0.1);
joe_feubli 6:3eaa420dc2f5 101
joe_feubli 7:44f2f9fd2eeb 102 led1 = 0;
joe_feubli 7:44f2f9fd2eeb 103 led2 = 0;
joe_feubli 7:44f2f9fd2eeb 104 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 105 led4 = 0;
joe_feubli 7:44f2f9fd2eeb 106
joe_feubli 6:3eaa420dc2f5 107 /* mySwitch1.mode(PullUp);
joe_feubli 6:3eaa420dc2f5 108 mySwitch2.mode(PullUp);
joe_feubli 6:3eaa420dc2f5 109 mySwitch3.mode(PullUp);
joe_feubli 6:3eaa420dc2f5 110 */
joe_feubli 2:0f4e77a22fd9 111 //************** Initialisierung **************//
joe_feubli 2:0f4e77a22fd9 112
joe_feubli 5:524e83ec5174 113 /* HOMING METHODEN:
joe_feubli 5:524e83ec5174 114 Homing Method 7 :Home Switch Positive Speed & Index
joe_feubli 5:524e83ec5174 115 Homing Method 11:Home Switch Negative Speed & Index
joe_feubli 5:524e83ec5174 116 Homing Method 23: Home Switch Positive Speed
joe_feubli 5:524e83ec5174 117 Homing Method 27: Home Switch Negative Speed
joe_feubli 6:3eaa420dc2f5 118 Homing Method -3: Current Threshold Positive Speed
joe_feubli 6:3eaa420dc2f5 119 Homing Method -4: Current Threshold Negative Speed
joe_feubli 6:3eaa420dc2f5 120
joe_feubli 5:524e83ec5174 121 */
joe_feubli 5:524e83ec5174 122 //Motortype 10: EC-Motor
joe_feubli 5:524e83ec5174 123
joe_feubli 6:3eaa420dc2f5 124 /* MyEpos1.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 6:3eaa420dc2f5 125 MyEpos1.SetHomingPar(-4, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 2:0f4e77a22fd9 126 MyEpos1.Reset();
joe_feubli 2:0f4e77a22fd9 127
joe_feubli 6:3eaa420dc2f5 128 MyEpos2.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 6:3eaa420dc2f5 129 MyEpos2.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 130 MyEpos2.Reset();
joe_feubli 2:0f4e77a22fd9 131
joe_feubli 6:3eaa420dc2f5 132 MyEpos3.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 6:3eaa420dc2f5 133 MyEpos3.SetHomingPar(-3, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 2:0f4e77a22fd9 134 MyEpos3.Reset();
joe_feubli 2:0f4e77a22fd9 135
joe_feubli 6:3eaa420dc2f5 136 MyEpos4.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 5:524e83ec5174 137 MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 138 MyEpos4.Reset();
joe_feubli 2:0f4e77a22fd9 139
joe_feubli 6:3eaa420dc2f5 140 MyEpos5.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
joe_feubli 6:3eaa420dc2f5 141 MyEpos5.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 142 MyEpos5.Reset();
joe_feubli 2:0f4e77a22fd9 143
joe_feubli 5:524e83ec5174 144 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 145 MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 146 MyEpos6.Reset();
joe_feubli 2:0f4e77a22fd9 147
joe_feubli 2:0f4e77a22fd9 148 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 149 MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 150 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 151 */
ODEM 0:aaf583a75b97 152
ODEM 0:aaf583a75b97 153 while(1){
joe_feubli 2:0f4e77a22fd9 154 switch (current_step){
joe_feubli 2:0f4e77a22fd9 155 case 1://HOMING:
joe_feubli 6:3eaa420dc2f5 156 led1 = 1;
joe_feubli 5:524e83ec5174 157
joe_feubli 6:3eaa420dc2f5 158 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 159 MyEpos2.Power(1);
joe_feubli 5:524e83ec5174 160 MyEpos3.Power(1);
joe_feubli 5:524e83ec5174 161 MyEpos4.Power(1);
joe_feubli 5:524e83ec5174 162 MyEpos5.Power(1);
joe_feubli 5:524e83ec5174 163 MyEpos6.Power(1);
joe_feubli 6:3eaa420dc2f5 164 MyEpos7.Power(1);
joe_feubli 6:3eaa420dc2f5 165
joe_feubli 6:3eaa420dc2f5 166 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 167
joe_feubli 7:44f2f9fd2eeb 168 if (node5_homed == false){
joe_feubli 7:44f2f9fd2eeb 169 MyEpos5.Homing(); //Kopf
joe_feubli 7:44f2f9fd2eeb 170 node5_homed = true;
joe_feubli 7:44f2f9fd2eeb 171 break;
joe_feubli 7:44f2f9fd2eeb 172 }
joe_feubli 7:44f2f9fd2eeb 173
joe_feubli 6:3eaa420dc2f5 174 if (node1_homed == false){
joe_feubli 6:3eaa420dc2f5 175 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 6:3eaa420dc2f5 176 node1_homed = true;
joe_feubli 6:3eaa420dc2f5 177 break;
joe_feubli 6:3eaa420dc2f5 178 }
joe_feubli 6:3eaa420dc2f5 179
joe_feubli 6:3eaa420dc2f5 180 if (node3_homed == false){
joe_feubli 6:3eaa420dc2f5 181 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 6:3eaa420dc2f5 182 node3_homed = true;
joe_feubli 6:3eaa420dc2f5 183 break;
joe_feubli 6:3eaa420dc2f5 184 }
joe_feubli 6:3eaa420dc2f5 185
joe_feubli 6:3eaa420dc2f5 186 if (node7_homed == false){
joe_feubli 6:3eaa420dc2f5 187 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 188 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 189 break;
joe_feubli 6:3eaa420dc2f5 190 }
joe_feubli 6:3eaa420dc2f5 191
joe_feubli 7:44f2f9fd2eeb 192 if (node1_homed == true and node2_homed == false and node5_homed == true){
joe_feubli 7:44f2f9fd2eeb 193 MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 194 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 7:44f2f9fd2eeb 195 MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
joe_feubli 7:44f2f9fd2eeb 196 wait(3);
joe_feubli 6:3eaa420dc2f5 197 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 198 break;
joe_feubli 2:0f4e77a22fd9 199 }
joe_feubli 6:3eaa420dc2f5 200
joe_feubli 6:3eaa420dc2f5 201 if (node3_homed == true and node4_homed == false){
joe_feubli 6:3eaa420dc2f5 202 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 203 node4_homed = true;
joe_feubli 6:3eaa420dc2f5 204 break;
joe_feubli 6:3eaa420dc2f5 205 }
joe_feubli 6:3eaa420dc2f5 206
joe_feubli 6:3eaa420dc2f5 207 if (node7_homed == true and node6_homed == false){
joe_feubli 6:3eaa420dc2f5 208 MyEpos6.Homing();//Bohrgestell
joe_feubli 6:3eaa420dc2f5 209 node6_homed = true;
joe_feubli 6:3eaa420dc2f5 210 break;
joe_feubli 6:3eaa420dc2f5 211 }
joe_feubli 6:3eaa420dc2f5 212 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){
joe_feubli 6:3eaa420dc2f5 213 current_step = 2;//BEREIT;
joe_feubli 7:44f2f9fd2eeb 214 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 215 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 216 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 217 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 218 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 219 MyEpos5.Power(0);
joe_feubli 2:0f4e77a22fd9 220 }
joe_feubli 6:3eaa420dc2f5 221
joe_feubli 7:44f2f9fd2eeb 222
joe_feubli 6:3eaa420dc2f5 223
joe_feubli 6:3eaa420dc2f5 224 led1 = 0;
joe_feubli 2:0f4e77a22fd9 225 break;
joe_feubli 2:0f4e77a22fd9 226
joe_feubli 2:0f4e77a22fd9 227
joe_feubli 2:0f4e77a22fd9 228 case 2://BEREIT:
joe_feubli 6:3eaa420dc2f5 229 led2 = 1;
joe_feubli 6:3eaa420dc2f5 230 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 6:3eaa420dc2f5 231 current_step = 6;
joe_feubli 7:44f2f9fd2eeb 232 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 233 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 234 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 235 MyEpos3.Power(1);
joe_feubli 6:3eaa420dc2f5 236 MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 237 MyEpos5.Power(1);
joe_feubli 6:3eaa420dc2f5 238 }
joe_feubli 6:3eaa420dc2f5 239 wait_ms(10);
joe_feubli 5:524e83ec5174 240 /* while (MyEpos6.GetDigIn(1)!=1) { //warten solange Buzzer nicht gedrückt
joe_feubli 2:0f4e77a22fd9 241 wait_ms(10);
joe_feubli 2:0f4e77a22fd9 242 }
joe_feubli 3:fbc9de097e4f 243 current_step = 3;//START_AUSFAHREN
joe_feubli 6:3eaa420dc2f5 244 current_step = 6; //Test
joe_feubli 6:3eaa420dc2f5 245 */ led2 = 0;
joe_feubli 2:0f4e77a22fd9 246 break;
ODEM 1:c53aafa72b36 247
joe_feubli 2:0f4e77a22fd9 248
joe_feubli 7:44f2f9fd2eeb 249 /* case 3://START_AUSFAHREN:
joe_feubli 6:3eaa420dc2f5 250 led3 = 1;
joe_feubli 6:3eaa420dc2f5 251 if(ausfahren == true){
joe_feubli 3:fbc9de097e4f 252 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 253 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 254 MyEpos2.MoveAbsolute(500,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 255 MyEpos4.MoveAbsolute(500,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 256 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 257 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 258 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 259 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 260 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 261 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 262 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 263 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 264 ausfahren = false;
joe_feubli 3:fbc9de097e4f 265 einfahren = true;
joe_feubli 3:fbc9de097e4f 266 current_step = 4;//START_EINFAHREN
joe_feubli 3:fbc9de097e4f 267 }
joe_feubli 3:fbc9de097e4f 268 }
joe_feubli 3:fbc9de097e4f 269 }
joe_feubli 3:fbc9de097e4f 270 }
joe_feubli 3:fbc9de097e4f 271 }
joe_feubli 3:fbc9de097e4f 272 }
joe_feubli 6:3eaa420dc2f5 273 led3 = 0;
joe_feubli 2:0f4e77a22fd9 274 break;
ODEM 1:c53aafa72b36 275
joe_feubli 3:fbc9de097e4f 276 case 4://Start_EINFAHREN
joe_feubli 6:3eaa420dc2f5 277 led4 = 1;
joe_feubli 6:3eaa420dc2f5 278 if(einfahren == true){
joe_feubli 3:fbc9de097e4f 279 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 280 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 281 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 282 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 283 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 284 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 285 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 286 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 287 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 288 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 289 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 290 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 291 einfahren = false;
joe_feubli 3:fbc9de097e4f 292 current_step = 2;//Bereit
joe_feubli 3:fbc9de097e4f 293 }
joe_feubli 3:fbc9de097e4f 294 }
joe_feubli 3:fbc9de097e4f 295 }
joe_feubli 3:fbc9de097e4f 296 }
joe_feubli 3:fbc9de097e4f 297 }
joe_feubli 3:fbc9de097e4f 298 }
joe_feubli 6:3eaa420dc2f5 299 led4 = 0;
joe_feubli 3:fbc9de097e4f 300 break;
joe_feubli 3:fbc9de097e4f 301
joe_feubli 7:44f2f9fd2eeb 302 */
joe_feubli 3:fbc9de097e4f 303 case 5://USV:
joe_feubli 7:44f2f9fd2eeb 304 led4 = 1;
joe_feubli 7:44f2f9fd2eeb 305 switch (usv_step){
joe_feubli 7:44f2f9fd2eeb 306 case 1:
joe_feubli 7:44f2f9fd2eeb 307 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 308 /* MyEpos1.Power(1);
joe_feubli 7:44f2f9fd2eeb 309 MyEpos2.Power(1);
joe_feubli 7:44f2f9fd2eeb 310 MyEpos3.Power(1);
joe_feubli 7:44f2f9fd2eeb 311 MyEpos4.Power(1);
joe_feubli 7:44f2f9fd2eeb 312 MyEpos5.Power(1);
joe_feubli 7:44f2f9fd2eeb 313 MyEpos6.Power(1);
joe_feubli 7:44f2f9fd2eeb 314 MyEpos7.Power(1);
joe_feubli 7:44f2f9fd2eeb 315 wait(1);
joe_feubli 7:44f2f9fd2eeb 316 */
joe_feubli 7:44f2f9fd2eeb 317 if (node1_homed == true){
joe_feubli 7:44f2f9fd2eeb 318 MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links
joe_feubli 7:44f2f9fd2eeb 319 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 320 }
joe_feubli 7:44f2f9fd2eeb 321 if (node3_homed == true){
joe_feubli 7:44f2f9fd2eeb 322 MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
joe_feubli 7:44f2f9fd2eeb 323 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 324 }
joe_feubli 7:44f2f9fd2eeb 325 /* if (node7_homed == true){
joe_feubli 7:44f2f9fd2eeb 326 MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 7:44f2f9fd2eeb 327 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 328 }
joe_feubli 7:44f2f9fd2eeb 329 */ wait(0.1);
joe_feubli 7:44f2f9fd2eeb 330 usv_step = 2;
joe_feubli 7:44f2f9fd2eeb 331 break;
joe_feubli 7:44f2f9fd2eeb 332
joe_feubli 7:44f2f9fd2eeb 333 case 2:
joe_feubli 7:44f2f9fd2eeb 334 if (node2_homed == true and MyEpos1.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 335 wait(1);
joe_feubli 7:44f2f9fd2eeb 336 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 7:44f2f9fd2eeb 337 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 338 }
joe_feubli 7:44f2f9fd2eeb 339 /* if (node4_homed == true and MyEpos3.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 340 wait(1);
joe_feubli 7:44f2f9fd2eeb 341 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 7:44f2f9fd2eeb 342 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 343 }
joe_feubli 7:44f2f9fd2eeb 344 if (node6_homed == true and MyEpos7.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 345 wait(1);
joe_feubli 7:44f2f9fd2eeb 346 MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 7:44f2f9fd2eeb 347 wait_ms(10);
ODEM 4:8fb30d6c2855 348 }
joe_feubli 7:44f2f9fd2eeb 349 */ wait(1);
joe_feubli 7:44f2f9fd2eeb 350 if (MyEpos2.TargetReached()==1 /*and MyEpos4.TargetReached()==1 and MyEpos6.TargetReached()==1*/){
joe_feubli 7:44f2f9fd2eeb 351 usv_step = 3;
joe_feubli 3:fbc9de097e4f 352 }
joe_feubli 7:44f2f9fd2eeb 353 break;
joe_feubli 7:44f2f9fd2eeb 354
joe_feubli 7:44f2f9fd2eeb 355 case 3:
joe_feubli 7:44f2f9fd2eeb 356 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 7:44f2f9fd2eeb 357 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 358 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 359 usv_step = 4;
joe_feubli 7:44f2f9fd2eeb 360 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 361 }
joe_feubli 7:44f2f9fd2eeb 362 break;
joe_feubli 7:44f2f9fd2eeb 363
joe_feubli 7:44f2f9fd2eeb 364 case 4:
joe_feubli 7:44f2f9fd2eeb 365 /*while (MyEpos5.TargetReached()==0){
joe_feubli 7:44f2f9fd2eeb 366 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 367 }*/
joe_feubli 7:44f2f9fd2eeb 368 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 369 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 370 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 371 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 372 MyEpos5.Power(0);
joe_feubli 7:44f2f9fd2eeb 373 MyEpos6.Power(0);
joe_feubli 7:44f2f9fd2eeb 374 MyEpos7.Power(0);
joe_feubli 7:44f2f9fd2eeb 375 usv_step = 5;
joe_feubli 7:44f2f9fd2eeb 376 break;
joe_feubli 7:44f2f9fd2eeb 377
joe_feubli 7:44f2f9fd2eeb 378 case 5:
joe_feubli 7:44f2f9fd2eeb 379 if(buffering == 1){
joe_feubli 7:44f2f9fd2eeb 380 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 381 } else {
joe_feubli 7:44f2f9fd2eeb 382 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 383 current_step=1;
joe_feubli 7:44f2f9fd2eeb 384 usv_step=1;
joe_feubli 7:44f2f9fd2eeb 385 ausfahren_step=1;
joe_feubli 7:44f2f9fd2eeb 386 einfahren_step=1;
joe_feubli 7:44f2f9fd2eeb 387 }
joe_feubli 7:44f2f9fd2eeb 388 /*if (buffering == 0){
joe_feubli 7:44f2f9fd2eeb 389 current_step = 1;
joe_feubli 7:44f2f9fd2eeb 390 usv_step = 1;
joe_feubli 7:44f2f9fd2eeb 391 }*/
joe_feubli 7:44f2f9fd2eeb 392 wait(1);
joe_feubli 7:44f2f9fd2eeb 393 break;
joe_feubli 3:fbc9de097e4f 394 }
joe_feubli 7:44f2f9fd2eeb 395 led4 = 0;
joe_feubli 2:0f4e77a22fd9 396 break;
ODEM 1:c53aafa72b36 397
joe_feubli 5:524e83ec5174 398 case 6://Test
joe_feubli 6:3eaa420dc2f5 399 // MyEpos5.Power(1);
joe_feubli 7:44f2f9fd2eeb 400 led3 = 1;
joe_feubli 6:3eaa420dc2f5 401 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 402 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 403 case 1:
joe_feubli 6:3eaa420dc2f5 404 MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 405 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 406 wait(1);
joe_feubli 6:3eaa420dc2f5 407 break;
joe_feubli 6:3eaa420dc2f5 408
joe_feubli 6:3eaa420dc2f5 409 case 2:
joe_feubli 6:3eaa420dc2f5 410 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 411 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 412 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 413 MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 414 ausfahren_step = 3;
joe_feubli 6:3eaa420dc2f5 415 wait(1);
joe_feubli 6:3eaa420dc2f5 416 }
joe_feubli 6:3eaa420dc2f5 417 break;
joe_feubli 6:3eaa420dc2f5 418
joe_feubli 6:3eaa420dc2f5 419 case 3:
joe_feubli 6:3eaa420dc2f5 420 if (MyEpos2.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 421 MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 422 MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 423
joe_feubli 6:3eaa420dc2f5 424 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 425 wait(1);
joe_feubli 6:3eaa420dc2f5 426 }
joe_feubli 6:3eaa420dc2f5 427 break;
joe_feubli 6:3eaa420dc2f5 428
joe_feubli 6:3eaa420dc2f5 429 case 4:
joe_feubli 6:3eaa420dc2f5 430 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 431 einfahren = true;
joe_feubli 6:3eaa420dc2f5 432 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 433 wait(1);
joe_feubli 6:3eaa420dc2f5 434 }
joe_feubli 6:3eaa420dc2f5 435 break;
joe_feubli 6:3eaa420dc2f5 436
joe_feubli 6:3eaa420dc2f5 437 }
joe_feubli 5:524e83ec5174 438 }
joe_feubli 6:3eaa420dc2f5 439
joe_feubli 6:3eaa420dc2f5 440 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 441 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 442 switch (einfahren_step){
joe_feubli 6:3eaa420dc2f5 443 case 1:
joe_feubli 6:3eaa420dc2f5 444 MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 445 MyEpos3.MoveAbsolute(-1000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 446 einfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 447 wait(1);
joe_feubli 6:3eaa420dc2f5 448 break;
joe_feubli 6:3eaa420dc2f5 449
joe_feubli 6:3eaa420dc2f5 450 case 2:
joe_feubli 6:3eaa420dc2f5 451 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 452 wait(1);
joe_feubli 6:3eaa420dc2f5 453 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 454 einfahren_step = 3;
joe_feubli 6:3eaa420dc2f5 455 wait(1);
joe_feubli 6:3eaa420dc2f5 456 }
joe_feubli 6:3eaa420dc2f5 457 break;
joe_feubli 6:3eaa420dc2f5 458
joe_feubli 6:3eaa420dc2f5 459 case 3:
joe_feubli 6:3eaa420dc2f5 460 if (MyEpos2.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 461 MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 462 wait(1);
joe_feubli 6:3eaa420dc2f5 463 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 464 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 465 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 466 MyEpos4.Power(0);
joe_feubli 6:3eaa420dc2f5 467 einfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 468 }
joe_feubli 6:3eaa420dc2f5 469 break;
joe_feubli 6:3eaa420dc2f5 470
joe_feubli 6:3eaa420dc2f5 471 case 4:
joe_feubli 6:3eaa420dc2f5 472 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 473 einfahren = false;
joe_feubli 6:3eaa420dc2f5 474 current_step = 2;
joe_feubli 6:3eaa420dc2f5 475 wait(1);
joe_feubli 6:3eaa420dc2f5 476 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 477 }
joe_feubli 6:3eaa420dc2f5 478 break;
joe_feubli 6:3eaa420dc2f5 479
joe_feubli 6:3eaa420dc2f5 480 }
joe_feubli 5:524e83ec5174 481 }
joe_feubli 7:44f2f9fd2eeb 482 led3 = 0;
ODEM 1:c53aafa72b36 483 break;
joe_feubli 7:44f2f9fd2eeb 484 }//case
joe_feubli 2:0f4e77a22fd9 485 }//while
joe_feubli 2:0f4e77a22fd9 486 }//main