mmotor / Mbed 2 deprecated MarsRover_ExoMars

Programm mit Ablauf

Dependencies:   EPOS2 mbed-rtos mbed

Fork of Projekt_Kugelbahn by mmotor

main.cpp@11:6e5ebe9aa84b, 2016-10-17 (annotated)

Committer:
joe_feubli
Date:
Mon Oct 17 10:22:25 2016 +0000
Revision:
11:6e5ebe9aa84b
Parent:
10:1c42bbd596dc
Child:
12:9e90d7a5a8c6
Alle Achsen von Planel in betrieb

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