mmotor / Mbed 2 deprecated MarsRover_ExoMars

Programm mit Ablauf

Dependencies:   EPOS2 mbed-rtos mbed

Fork of Projekt_Kugelbahn by mmotor

main.cpp@9:039318b9096e, 2016-10-14 (annotated)

Committer:
joe_feubli
Date:
Fri Oct 14 13:41:28 2016 +0000
Revision:
9:039318b9096e
Parent:
8:98d4028cdea6
Child:
10:1c42bbd596dc
Problem mit ablauf

;

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 7:44f2f9fd2eeb 12 DigitalIn usv_ready(p23);
joe_feubli 7:44f2f9fd2eeb 13 InterruptIn buffering(p24);
joe_feubli 7:44f2f9fd2eeb 14 DigitalIn replace_battery(p25);
joe_feubli 6:3eaa420dc2f5 15
joe_feubli 6:3eaa420dc2f5 16 DigitalOut led1(LED1);
joe_feubli 6:3eaa420dc2f5 17 DigitalOut led2(LED2);
joe_feubli 6:3eaa420dc2f5 18 DigitalOut led3(LED3);
joe_feubli 6:3eaa420dc2f5 19 DigitalOut led4(LED4);
joe_feubli 6:3eaa420dc2f5 20
joe_feubli 6:3eaa420dc2f5 21 bool node1_homed = false;
joe_feubli 6:3eaa420dc2f5 22 bool node2_homed = false;
joe_feubli 6:3eaa420dc2f5 23 bool node3_homed = false;
joe_feubli 6:3eaa420dc2f5 24 bool node4_homed = true;
joe_feubli 6:3eaa420dc2f5 25 bool node5_homed = false;
joe_feubli 6:3eaa420dc2f5 26 bool node6_homed = true;
joe_feubli 6:3eaa420dc2f5 27 bool node7_homed = true;
joe_feubli 6:3eaa420dc2f5 28
joe_feubli 6:3eaa420dc2f5 29 int homing = false;
joe_feubli 6:3eaa420dc2f5 30 int ready = false;
joe_feubli 3:fbc9de097e4f 31 int ausfahren = false;
joe_feubli 6:3eaa420dc2f5 32 int einfahren = false;
joe_feubli 6:3eaa420dc2f5 33 int usv = false;
joe_feubli 6:3eaa420dc2f5 34 int current_step = 1; //HOMING
joe_feubli 6:3eaa420dc2f5 35 int ausfahren_step = 1;
joe_feubli 7:44f2f9fd2eeb 36 int einfahren_step = 1;
joe_feubli 7:44f2f9fd2eeb 37 int usv_step = 1;
joe_feubli 6:3eaa420dc2f5 38
joe_feubli 6:3eaa420dc2f5 39 void interrupt_usv(){
joe_feubli 6:3eaa420dc2f5 40 current_step = 5;
joe_feubli 7:44f2f9fd2eeb 41 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 42 }
ODEM 4:8fb30d6c2855 43
joe_feubli 2:0f4e77a22fd9 44 int main(){ ///////// Main
joe_feubli 7:44f2f9fd2eeb 45 led1 = 1;
ODEM 0:aaf583a75b97 46 pc.baud(9600);
ODEM 1:c53aafa72b36 47
joe_feubli 8:98d4028cdea6 48 buffering.rise(&interrupt_usv); //Interrupt für USV
joe_feubli 7:44f2f9fd2eeb 49
joe_feubli 9:039318b9096e 50 wait(0.5); //Wartezeit bis Epos2 24/5 aufgestartet ist mit Lade-LED
joe_feubli 7:44f2f9fd2eeb 51 led2 = 1;
joe_feubli 7:44f2f9fd2eeb 52 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 53 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 54 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 55 led4 = 1;
joe_feubli 9:039318b9096e 56 wait(0.5);
ODEM 0:aaf583a75b97 57
ODEM 0:aaf583a75b97 58 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 59
ODEM 0:aaf583a75b97 60 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 61 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 62
joe_feubli 2:0f4e77a22fd9 63 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 64 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 65 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 66 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 67 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 68 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 69 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 70
ODEM 0:aaf583a75b97 71 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 2:0f4e77a22fd9 72
joe_feubli 6:3eaa420dc2f5 73 MyEpos1.Reset();
joe_feubli 6:3eaa420dc2f5 74 MyEpos2.Reset();
joe_feubli 6:3eaa420dc2f5 75 MyEpos3.Reset();
joe_feubli 6:3eaa420dc2f5 76 MyEpos4.Reset();
joe_feubli 6:3eaa420dc2f5 77 MyEpos5.Reset();
joe_feubli 6:3eaa420dc2f5 78 MyEpos6.Reset();
joe_feubli 6:3eaa420dc2f5 79 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 80 wait(0.1);
joe_feubli 6:3eaa420dc2f5 81
joe_feubli 7:44f2f9fd2eeb 82 led1 = 0;
joe_feubli 7:44f2f9fd2eeb 83 led2 = 0;
joe_feubli 7:44f2f9fd2eeb 84 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 85 led4 = 0;
joe_feubli 8:98d4028cdea6 86 MyEpos5.SetDigOut(4, 1);
joe_feubli 7:44f2f9fd2eeb 87
ODEM 0:aaf583a75b97 88
ODEM 0:aaf583a75b97 89 while(1){
joe_feubli 2:0f4e77a22fd9 90 switch (current_step){
joe_feubli 2:0f4e77a22fd9 91 case 1://HOMING:
joe_feubli 6:3eaa420dc2f5 92 led1 = 1;
joe_feubli 5:524e83ec5174 93
joe_feubli 6:3eaa420dc2f5 94 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 95 MyEpos2.Power(1);
joe_feubli 5:524e83ec5174 96 MyEpos3.Power(1);
joe_feubli 5:524e83ec5174 97 MyEpos4.Power(1);
joe_feubli 5:524e83ec5174 98 MyEpos5.Power(1);
joe_feubli 5:524e83ec5174 99 MyEpos6.Power(1);
joe_feubli 6:3eaa420dc2f5 100 MyEpos7.Power(1);
joe_feubli 6:3eaa420dc2f5 101
joe_feubli 6:3eaa420dc2f5 102 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 103
joe_feubli 7:44f2f9fd2eeb 104 if (node5_homed == false){
joe_feubli 7:44f2f9fd2eeb 105 MyEpos5.Homing(); //Kopf
joe_feubli 7:44f2f9fd2eeb 106 node5_homed = true;
joe_feubli 7:44f2f9fd2eeb 107 break;
joe_feubli 7:44f2f9fd2eeb 108 }
joe_feubli 7:44f2f9fd2eeb 109
joe_feubli 6:3eaa420dc2f5 110 if (node1_homed == false){
joe_feubli 6:3eaa420dc2f5 111 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 6:3eaa420dc2f5 112 node1_homed = true;
joe_feubli 6:3eaa420dc2f5 113 break;
joe_feubli 6:3eaa420dc2f5 114 }
joe_feubli 6:3eaa420dc2f5 115
joe_feubli 6:3eaa420dc2f5 116 if (node3_homed == false){
joe_feubli 6:3eaa420dc2f5 117 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 6:3eaa420dc2f5 118 node3_homed = true;
joe_feubli 6:3eaa420dc2f5 119 break;
joe_feubli 6:3eaa420dc2f5 120 }
joe_feubli 6:3eaa420dc2f5 121
joe_feubli 6:3eaa420dc2f5 122 if (node7_homed == false){
joe_feubli 6:3eaa420dc2f5 123 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 124 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 125 break;
joe_feubli 6:3eaa420dc2f5 126 }
joe_feubli 6:3eaa420dc2f5 127
joe_feubli 7:44f2f9fd2eeb 128 if (node1_homed == true and node2_homed == false and node5_homed == true){
joe_feubli 7:44f2f9fd2eeb 129 MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 130 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 7:44f2f9fd2eeb 131 MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
joe_feubli 7:44f2f9fd2eeb 132 wait(3);
joe_feubli 6:3eaa420dc2f5 133 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 134 break;
joe_feubli 2:0f4e77a22fd9 135 }
joe_feubli 6:3eaa420dc2f5 136
joe_feubli 6:3eaa420dc2f5 137 if (node3_homed == true and node4_homed == false){
joe_feubli 6:3eaa420dc2f5 138 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 139 node4_homed = true;
joe_feubli 6:3eaa420dc2f5 140 break;
joe_feubli 6:3eaa420dc2f5 141 }
joe_feubli 6:3eaa420dc2f5 142
joe_feubli 6:3eaa420dc2f5 143 if (node7_homed == true and node6_homed == false){
joe_feubli 6:3eaa420dc2f5 144 MyEpos6.Homing();//Bohrgestell
joe_feubli 6:3eaa420dc2f5 145 node6_homed = true;
joe_feubli 6:3eaa420dc2f5 146 break;
joe_feubli 6:3eaa420dc2f5 147 }
joe_feubli 6:3eaa420dc2f5 148 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 149 current_step = 2;//BEREIT;
joe_feubli 7:44f2f9fd2eeb 150 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 151 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 152 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 153 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 154 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 155 MyEpos5.Power(0);
joe_feubli 9:039318b9096e 156 MyEpos6.Power(0);
joe_feubli 9:039318b9096e 157 MyEpos7.Power(0);
joe_feubli 2:0f4e77a22fd9 158 }
joe_feubli 8:98d4028cdea6 159
joe_feubli 6:3eaa420dc2f5 160 led1 = 0;
joe_feubli 2:0f4e77a22fd9 161 break;
joe_feubli 2:0f4e77a22fd9 162
joe_feubli 2:0f4e77a22fd9 163
joe_feubli 2:0f4e77a22fd9 164 case 2://BEREIT:
joe_feubli 6:3eaa420dc2f5 165 led2 = 1;
joe_feubli 8:98d4028cdea6 166 MyEpos5.SetDigOut(4,0);
joe_feubli 6:3eaa420dc2f5 167 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 8:98d4028cdea6 168 MyEpos5.SetDigOut(4,1);
joe_feubli 9:039318b9096e 169 current_step = 3; //ABLAUF
joe_feubli 7:44f2f9fd2eeb 170 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 171 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 172 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 173 MyEpos3.Power(1);
joe_feubli 9:039318b9096e 174 // MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 175 MyEpos5.Power(1);
joe_feubli 9:039318b9096e 176 // MyEpos6.Power(1);
joe_feubli 9:039318b9096e 177 // MyEpos7.Power(1);
joe_feubli 6:3eaa420dc2f5 178 }
joe_feubli 9:039318b9096e 179 wait(1);
joe_feubli 9:039318b9096e 180 led2 = 0;
joe_feubli 2:0f4e77a22fd9 181 break;
ODEM 1:c53aafa72b36 182
joe_feubli 2:0f4e77a22fd9 183
joe_feubli 9:039318b9096e 184 case 3://ABLAUF:
joe_feubli 7:44f2f9fd2eeb 185 led3 = 1;
joe_feubli 6:3eaa420dc2f5 186 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 187 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 188 case 1:
joe_feubli 6:3eaa420dc2f5 189 MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 190 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 191 wait(1);
joe_feubli 6:3eaa420dc2f5 192 break;
joe_feubli 6:3eaa420dc2f5 193
joe_feubli 6:3eaa420dc2f5 194 case 2:
joe_feubli 6:3eaa420dc2f5 195 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 196 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 197 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 198 MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Aussen Links
joe_feubli 9:039318b9096e 199 wait(0.1);
joe_feubli 6:3eaa420dc2f5 200 ausfahren_step = 3;
joe_feubli 6:3eaa420dc2f5 201 }
joe_feubli 6:3eaa420dc2f5 202 break;
joe_feubli 6:3eaa420dc2f5 203
joe_feubli 6:3eaa420dc2f5 204 case 3:
joe_feubli 6:3eaa420dc2f5 205 if (MyEpos2.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 206 MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 207 MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 208
joe_feubli 6:3eaa420dc2f5 209 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 210 wait(1);
joe_feubli 6:3eaa420dc2f5 211 }
joe_feubli 6:3eaa420dc2f5 212 break;
joe_feubli 6:3eaa420dc2f5 213
joe_feubli 6:3eaa420dc2f5 214 case 4:
joe_feubli 6:3eaa420dc2f5 215 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 216 einfahren = true;
joe_feubli 6:3eaa420dc2f5 217 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 218 wait(1);
joe_feubli 6:3eaa420dc2f5 219 }
joe_feubli 6:3eaa420dc2f5 220 break;
joe_feubli 9:039318b9096e 221 }//switch ausfahren_step
joe_feubli 9:039318b9096e 222 }//if (einfahren == false)
joe_feubli 6:3eaa420dc2f5 223
joe_feubli 6:3eaa420dc2f5 224 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 225 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 226 switch (einfahren_step){
joe_feubli 6:3eaa420dc2f5 227 case 1:
joe_feubli 8:98d4028cdea6 228 MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 229 MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 230 einfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 231 wait(1);
joe_feubli 6:3eaa420dc2f5 232 break;
joe_feubli 6:3eaa420dc2f5 233
joe_feubli 6:3eaa420dc2f5 234 case 2:
joe_feubli 6:3eaa420dc2f5 235 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 9:039318b9096e 236 // wait(1);
joe_feubli 8:98d4028cdea6 237 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 238 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 239 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 240 einfahren_step = 3;
joe_feubli 6:3eaa420dc2f5 241 wait(1);
joe_feubli 6:3eaa420dc2f5 242 }
joe_feubli 6:3eaa420dc2f5 243 break;
joe_feubli 6:3eaa420dc2f5 244
joe_feubli 6:3eaa420dc2f5 245 case 3:
joe_feubli 6:3eaa420dc2f5 246 if (MyEpos2.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 247 MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 248 wait(1);
joe_feubli 6:3eaa420dc2f5 249 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 250 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 251 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 252 MyEpos4.Power(0);
joe_feubli 6:3eaa420dc2f5 253 einfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 254 }
joe_feubli 6:3eaa420dc2f5 255 break;
joe_feubli 6:3eaa420dc2f5 256
joe_feubli 6:3eaa420dc2f5 257 case 4:
joe_feubli 6:3eaa420dc2f5 258 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 259 einfahren = false;
joe_feubli 6:3eaa420dc2f5 260 current_step = 2;
joe_feubli 6:3eaa420dc2f5 261 wait(1);
joe_feubli 6:3eaa420dc2f5 262 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 263 }
joe_feubli 9:039318b9096e 264 break;
joe_feubli 9:039318b9096e 265 }//switch einfahren_step
joe_feubli 9:039318b9096e 266 }//if (einfahren == true)
joe_feubli 7:44f2f9fd2eeb 267 led3 = 0;
ODEM 1:c53aafa72b36 268 break;
joe_feubli 9:039318b9096e 269
joe_feubli 9:039318b9096e 270 case 5://USV:
joe_feubli 9:039318b9096e 271 led4 = 1;
joe_feubli 9:039318b9096e 272 MyEpos5.SetDigOut(4,1);
joe_feubli 9:039318b9096e 273 switch (usv_step){
joe_feubli 9:039318b9096e 274 case 1:
joe_feubli 9:039318b9096e 275 MyEpos1.Power(1);
joe_feubli 9:039318b9096e 276 MyEpos2.Power(1);
joe_feubli 9:039318b9096e 277 MyEpos3.Power(1);
joe_feubli 9:039318b9096e 278 MyEpos4.Power(1);
joe_feubli 9:039318b9096e 279 MyEpos5.Power(1);
joe_feubli 9:039318b9096e 280 MyEpos6.Power(1);
joe_feubli 9:039318b9096e 281 MyEpos7.Power(1);
joe_feubli 9:039318b9096e 282 wait(0.1);
joe_feubli 9:039318b9096e 283
joe_feubli 9:039318b9096e 284 MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
joe_feubli 9:039318b9096e 285 MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
joe_feubli 9:039318b9096e 286 // MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 9:039318b9096e 287 wait(0.1);
joe_feubli 9:039318b9096e 288 usv_step = 2;
joe_feubli 9:039318b9096e 289 break;
joe_feubli 9:039318b9096e 290
joe_feubli 9:039318b9096e 291 case 2:
joe_feubli 9:039318b9096e 292 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1 /*and MyEpos7.TargetReached()==1*/){
joe_feubli 9:039318b9096e 293 usv_step = 3;
joe_feubli 9:039318b9096e 294 wait_ms(10);
joe_feubli 9:039318b9096e 295 }
joe_feubli 9:039318b9096e 296 break;
joe_feubli 9:039318b9096e 297
joe_feubli 9:039318b9096e 298 case 3:
joe_feubli 9:039318b9096e 299 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 9:039318b9096e 300 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 9:039318b9096e 301 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 9:039318b9096e 302 // MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 9:039318b9096e 303 // MyEpos6.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 9:039318b9096e 304 wait(0.1);
joe_feubli 9:039318b9096e 305
joe_feubli 9:039318b9096e 306 if (MyEpos2.TargetReached()==1 /*and MyEpos4.TargetReached()==1*/){
joe_feubli 9:039318b9096e 307 usv_step = 4;
joe_feubli 9:039318b9096e 308 wait_ms(10);
joe_feubli 9:039318b9096e 309 }
joe_feubli 9:039318b9096e 310 break;
joe_feubli 9:039318b9096e 311
joe_feubli 9:039318b9096e 312 case 4:
joe_feubli 9:039318b9096e 313 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 9:039318b9096e 314 wait(0.1);
joe_feubli 9:039318b9096e 315 if (MyEpos5.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
joe_feubli 9:039318b9096e 316 usv_step = 5;
joe_feubli 9:039318b9096e 317 wait(0.1);
joe_feubli 9:039318b9096e 318 }
joe_feubli 9:039318b9096e 319 break;
joe_feubli 9:039318b9096e 320
joe_feubli 9:039318b9096e 321 case 5:
joe_feubli 9:039318b9096e 322 MyEpos1.Power(0);
joe_feubli 9:039318b9096e 323 MyEpos2.Power(0);
joe_feubli 9:039318b9096e 324 MyEpos3.Power(0);
joe_feubli 9:039318b9096e 325 MyEpos4.Power(0);
joe_feubli 9:039318b9096e 326 MyEpos5.Power(0);
joe_feubli 9:039318b9096e 327 MyEpos6.Power(0);
joe_feubli 9:039318b9096e 328 MyEpos7.Power(0);
joe_feubli 9:039318b9096e 329 usv_step = 6;
joe_feubli 9:039318b9096e 330 break;
joe_feubli 9:039318b9096e 331
joe_feubli 9:039318b9096e 332 case 6:
joe_feubli 9:039318b9096e 333 if(buffering == 1){
joe_feubli 9:039318b9096e 334 led3 = 1;
joe_feubli 9:039318b9096e 335 } else {
joe_feubli 9:039318b9096e 336 led3 = 0;
joe_feubli 9:039318b9096e 337 current_step=1;
joe_feubli 9:039318b9096e 338 usv_step=1;
joe_feubli 9:039318b9096e 339 ausfahren_step=1;
joe_feubli 9:039318b9096e 340 einfahren_step=1;
joe_feubli 9:039318b9096e 341 }
joe_feubli 9:039318b9096e 342 wait(1);
joe_feubli 9:039318b9096e 343 break;
joe_feubli 9:039318b9096e 344 }
joe_feubli 9:039318b9096e 345 led4 = 0;
joe_feubli 9:039318b9096e 346 break;
joe_feubli 9:039318b9096e 347
joe_feubli 9:039318b9096e 348
joe_feubli 9:039318b9096e 349 case 6://Test
joe_feubli 9:039318b9096e 350 break;
joe_feubli 9:039318b9096e 351
joe_feubli 9:039318b9096e 352 }//switch current_step
joe_feubli 2:0f4e77a22fd9 353 }//while
joe_feubli 2:0f4e77a22fd9 354 }//main