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@14:bee23de5dff0, 2016-11-03 (annotated)

Committer:
joe_feubli
Date:
Thu Nov 03 10:36:46 2016 +0000
Revision:
14:bee23de5dff0
Parent:
13:cc43e546f310
Child:
15:c31c862632c0
Child:
16:e5343d6c9fb2
Aktuellste Version MarsRover mit Bohrkopf und Bohrer

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 7:44f2f9fd2eeb 13 DigitalIn usv_ready(p23);
joe_feubli 7:44f2f9fd2eeb 14 InterruptIn buffering(p24);
joe_feubli 7:44f2f9fd2eeb 15 DigitalIn replace_battery(p25);
joe_feubli 6:3eaa420dc2f5 16
joe_feubli 6:3eaa420dc2f5 17 DigitalOut led1(LED1);
joe_feubli 6:3eaa420dc2f5 18 DigitalOut led2(LED2);
joe_feubli 6:3eaa420dc2f5 19 DigitalOut led3(LED3);
joe_feubli 6:3eaa420dc2f5 20 DigitalOut led4(LED4);
joe_feubli 6:3eaa420dc2f5 21
joe_feubli 13:cc43e546f310 22 bool epos1_homing = false;
joe_feubli 13:cc43e546f310 23 bool epos3_homing = false;
joe_feubli 13:cc43e546f310 24
joe_feubli 6:3eaa420dc2f5 25 bool node1_homed = false;
joe_feubli 6:3eaa420dc2f5 26 bool node2_homed = false;
joe_feubli 6:3eaa420dc2f5 27 bool node3_homed = false;
joe_feubli 11:6e5ebe9aa84b 28 bool node4_homed = false;
joe_feubli 6:3eaa420dc2f5 29 bool node5_homed = false;
joe_feubli 14:bee23de5dff0 30 bool node6_homed = false;
joe_feubli 14:bee23de5dff0 31 bool node7_homed = false;
joe_feubli 6:3eaa420dc2f5 32
joe_feubli 12:9e90d7a5a8c6 33 bool Power_ein = true;
joe_feubli 12:9e90d7a5a8c6 34 bool node5_grundposition = false;
joe_feubli 12:9e90d7a5a8c6 35
joe_feubli 13:cc43e546f310 36 bool ausfahren = false;
joe_feubli 13:cc43e546f310 37 bool einfahren = false;
joe_feubli 13:cc43e546f310 38 bool usv = false;
joe_feubli 6:3eaa420dc2f5 39 int current_step = 1; //HOMING
joe_feubli 6:3eaa420dc2f5 40 int ausfahren_step = 1;
joe_feubli 7:44f2f9fd2eeb 41 int einfahren_step = 1;
joe_feubli 12:9e90d7a5a8c6 42 int usv_step = 1;
joe_feubli 13:cc43e546f310 43 int ErrorState1;
joe_feubli 13:cc43e546f310 44 int ErrorState2;
joe_feubli 13:cc43e546f310 45 int ErrorState3;
joe_feubli 13:cc43e546f310 46 int ErrorState4;
joe_feubli 13:cc43e546f310 47 int ErrorState5;
joe_feubli 13:cc43e546f310 48 int ErrorState6;
joe_feubli 13:cc43e546f310 49 int ErrorState7;
joe_feubli 14:bee23de5dff0 50 float speed = 1.5;
joe_feubli 6:3eaa420dc2f5 51
joe_feubli 13:cc43e546f310 52 void interrupt_usv(){ //Funktion, wird aufgeruffen durch den interrupt des USV's
joe_feubli 6:3eaa420dc2f5 53 current_step = 5;
joe_feubli 7:44f2f9fd2eeb 54 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 55 }
ODEM 4:8fb30d6c2855 56
joe_feubli 2:0f4e77a22fd9 57 int main(){ ///////// Main
joe_feubli 7:44f2f9fd2eeb 58 led1 = 1;
ODEM 0:aaf583a75b97 59 pc.baud(9600);
ODEM 1:c53aafa72b36 60
joe_feubli 8:98d4028cdea6 61 buffering.rise(&interrupt_usv); //Interrupt für USV
joe_feubli 7:44f2f9fd2eeb 62
joe_feubli 12:9e90d7a5a8c6 63 wait(0.5); //Wartezeit bis Epos2 24/5 aufgestartet ist
joe_feubli 7:44f2f9fd2eeb 64 led2 = 1;
joe_feubli 7:44f2f9fd2eeb 65 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 66 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 67 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 68 led4 = 1;
joe_feubli 10:1c42bbd596dc 69 wait(0.5);
joe_feubli 10:1c42bbd596dc 70
joe_feubli 10:1c42bbd596dc 71 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 72
ODEM 0:aaf583a75b97 73 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 74
ODEM 0:aaf583a75b97 75 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 76 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 77
joe_feubli 2:0f4e77a22fd9 78 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 79 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 80 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 81 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 82 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 83 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 84 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 85
ODEM 0:aaf583a75b97 86 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 12:9e90d7a5a8c6 87 wait(0.1);
joe_feubli 2:0f4e77a22fd9 88
joe_feubli 6:3eaa420dc2f5 89 MyEpos1.Reset();
joe_feubli 6:3eaa420dc2f5 90 MyEpos2.Reset();
joe_feubli 6:3eaa420dc2f5 91 MyEpos3.Reset();
joe_feubli 6:3eaa420dc2f5 92 MyEpos4.Reset();
joe_feubli 6:3eaa420dc2f5 93 MyEpos5.Reset();
joe_feubli 6:3eaa420dc2f5 94 MyEpos6.Reset();
joe_feubli 6:3eaa420dc2f5 95 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 96 wait(0.1);
joe_feubli 6:3eaa420dc2f5 97
joe_feubli 7:44f2f9fd2eeb 98 led1 = 0;
joe_feubli 7:44f2f9fd2eeb 99 led2 = 0;
joe_feubli 7:44f2f9fd2eeb 100 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 101 led4 = 0;
joe_feubli 8:98d4028cdea6 102 MyEpos5.SetDigOut(4, 1);
joe_feubli 13:cc43e546f310 103
joe_feubli 11:6e5ebe9aa84b 104 while(1){
joe_feubli 12:9e90d7a5a8c6 105
joe_feubli 12:9e90d7a5a8c6 106
joe_feubli 11:6e5ebe9aa84b 107 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 108 current_step = 7; //FEHLER
joe_feubli 14:bee23de5dff0 109 MyEpos1.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 110 MyEpos2.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 111 MyEpos3.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 112 MyEpos4.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 113 MyEpos5.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 114 wait(0.1);
joe_feubli 13:cc43e546f310 115 ErrorState1 = MyEpos1.GetError();
joe_feubli 13:cc43e546f310 116 ErrorState2 = MyEpos2.GetError();
joe_feubli 13:cc43e546f310 117 ErrorState3 = MyEpos3.GetError();
joe_feubli 13:cc43e546f310 118 ErrorState4 = MyEpos4.GetError();
joe_feubli 13:cc43e546f310 119 ErrorState5 = MyEpos5.GetError();
joe_feubli 12:9e90d7a5a8c6 120 wait(0.1);
joe_feubli 13:cc43e546f310 121 pc.printf("ErrorState Panels/Kopf: Panel Aussen Linkss=%d, Panel Innen Links=%d, Panel Aussen Rechts=%d, Panel Innen Rechts=%d, Kopf=%d\r\n", ErrorState1, ErrorState2, ErrorState3, ErrorState4, ErrorState5);
joe_feubli 11:6e5ebe9aa84b 122 }
joe_feubli 11:6e5ebe9aa84b 123
joe_feubli 14:bee23de5dff0 124 if(MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0){
joe_feubli 14:bee23de5dff0 125 current_step = 6; //FEHLER
joe_feubli 14:bee23de5dff0 126 MyEpos1.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 127 MyEpos2.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 128 MyEpos3.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 129 wait(0.1);
joe_feubli 13:cc43e546f310 130 ErrorState6 = MyEpos6.GetError();
joe_feubli 13:cc43e546f310 131 ErrorState7 = MyEpos7.GetError();
joe_feubli 13:cc43e546f310 132 wait(0.1);
joe_feubli 13:cc43e546f310 133 pc.printf("Bohrgestell/Bohrer ErrorState: Bohrgestell=%d, Bohrer=%d\r\n", ErrorState6, ErrorState7);
joe_feubli 13:cc43e546f310 134 }
joe_feubli 13:cc43e546f310 135
joe_feubli 14:bee23de5dff0 136 switch (current_step){
joe_feubli 2:0f4e77a22fd9 137 case 1://HOMING:
joe_feubli 12:9e90d7a5a8c6 138
joe_feubli 6:3eaa420dc2f5 139 led1 = 1;
joe_feubli 12:9e90d7a5a8c6 140 if (Power_ein == true){
joe_feubli 12:9e90d7a5a8c6 141 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 142 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 143 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 144 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 145 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 146 MyEpos6.Power(1);
joe_feubli 12:9e90d7a5a8c6 147 MyEpos7.Power(1);
joe_feubli 12:9e90d7a5a8c6 148 Power_ein = false;
joe_feubli 12:9e90d7a5a8c6 149 }
joe_feubli 6:3eaa420dc2f5 150
joe_feubli 6:3eaa420dc2f5 151 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 152
joe_feubli 6:3eaa420dc2f5 153 if (node1_homed == false){
joe_feubli 13:cc43e546f310 154 if(epos1_homing == false){
joe_feubli 13:cc43e546f310 155 MyEpos1.MoveVelocity(-400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
joe_feubli 13:cc43e546f310 156 epos1_homing = true;
joe_feubli 13:cc43e546f310 157 }
joe_feubli 14:bee23de5dff0 158 if(MyEpos1.GetDigIn(4) == 1){
joe_feubli 13:cc43e546f310 159 pc.printf("Homing1\n");
joe_feubli 13:cc43e546f310 160 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 13:cc43e546f310 161 node1_homed = true;
joe_feubli 13:cc43e546f310 162 break;
joe_feubli 13:cc43e546f310 163 }
joe_feubli 6:3eaa420dc2f5 164 break;
joe_feubli 6:3eaa420dc2f5 165 }
joe_feubli 12:9e90d7a5a8c6 166
joe_feubli 6:3eaa420dc2f5 167
joe_feubli 6:3eaa420dc2f5 168 if (node3_homed == false){
joe_feubli 13:cc43e546f310 169 if (epos3_homing == false){
joe_feubli 13:cc43e546f310 170 MyEpos3.MoveVelocity(400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
joe_feubli 13:cc43e546f310 171 epos3_homing = true;
joe_feubli 13:cc43e546f310 172 }
joe_feubli 14:bee23de5dff0 173 if(MyEpos3.GetDigIn(4) == 1){
joe_feubli 13:cc43e546f310 174 pc.printf("Homing3\n");
joe_feubli 13:cc43e546f310 175 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 13:cc43e546f310 176 node3_homed = true;
joe_feubli 13:cc43e546f310 177 }
joe_feubli 6:3eaa420dc2f5 178 break;
joe_feubli 6:3eaa420dc2f5 179 }
joe_feubli 6:3eaa420dc2f5 180
joe_feubli 14:bee23de5dff0 181 if (node5_homed == false){
joe_feubli 14:bee23de5dff0 182 pc.printf("Homing5\n");
joe_feubli 14:bee23de5dff0 183 MyEpos5.Homing(); //Kopf
joe_feubli 14:bee23de5dff0 184 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 14:bee23de5dff0 185 pc.printf("Fahren auf -60000\n");
joe_feubli 14:bee23de5dff0 186 while(MyEpos5.TargetReached()!=1){
joe_feubli 14:bee23de5dff0 187 if(MyEpos5.GetError() == 1){
joe_feubli 14:bee23de5dff0 188 break;
joe_feubli 14:bee23de5dff0 189 }
joe_feubli 14:bee23de5dff0 190 }
joe_feubli 14:bee23de5dff0 191 pc.printf("Target reached\n");
joe_feubli 14:bee23de5dff0 192 node5_homed = true;
joe_feubli 14:bee23de5dff0 193 break;
joe_feubli 14:bee23de5dff0 194 }
joe_feubli 14:bee23de5dff0 195
joe_feubli 6:3eaa420dc2f5 196 if (node7_homed == false){
joe_feubli 12:9e90d7a5a8c6 197 pc.printf("Homing7\n");
joe_feubli 6:3eaa420dc2f5 198 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 199 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 200 break;
joe_feubli 6:3eaa420dc2f5 201 }
joe_feubli 6:3eaa420dc2f5 202
joe_feubli 13:cc43e546f310 203 if (node1_homed == true and node2_homed == false){
joe_feubli 13:cc43e546f310 204 if (node5_grundposition == false){
joe_feubli 13:cc43e546f310 205 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 13:cc43e546f310 206 }
joe_feubli 12:9e90d7a5a8c6 207 pc.printf("Homing2\n");
joe_feubli 6:3eaa420dc2f5 208 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 6:3eaa420dc2f5 209 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 210 break;
joe_feubli 2:0f4e77a22fd9 211 }
joe_feubli 6:3eaa420dc2f5 212
joe_feubli 6:3eaa420dc2f5 213 if (node3_homed == true and node4_homed == false){
joe_feubli 12:9e90d7a5a8c6 214 pc.printf("Homing4\n");
joe_feubli 6:3eaa420dc2f5 215 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 216 node4_homed = true;
joe_feubli 14:bee23de5dff0 217 MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 218 wait(0.1);
joe_feubli 6:3eaa420dc2f5 219 break;
joe_feubli 6:3eaa420dc2f5 220 }
joe_feubli 12:9e90d7a5a8c6 221
joe_feubli 12:9e90d7a5a8c6 222
joe_feubli 6:3eaa420dc2f5 223
joe_feubli 6:3eaa420dc2f5 224 if (node7_homed == true and node6_homed == false){
joe_feubli 12:9e90d7a5a8c6 225 pc.printf("Homing6\n");
joe_feubli 6:3eaa420dc2f5 226 MyEpos6.Homing();//Bohrgestell
joe_feubli 6:3eaa420dc2f5 227 node6_homed = true;
joe_feubli 6:3eaa420dc2f5 228 break;
joe_feubli 6:3eaa420dc2f5 229 }
joe_feubli 12:9e90d7a5a8c6 230
joe_feubli 13:cc43e546f310 231 if (node2_homed == true and node4_homed == true and node5_grundposition == false){
joe_feubli 14:bee23de5dff0 232 //MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 233 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 234 while(MyEpos5.TargetReached() != 1){
joe_feubli 13:cc43e546f310 235 if(MyEpos5.GetError() == 1){
joe_feubli 13:cc43e546f310 236 break;
joe_feubli 13:cc43e546f310 237 }
joe_feubli 12:9e90d7a5a8c6 238 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 239 }
joe_feubli 12:9e90d7a5a8c6 240 node5_grundposition = true;
joe_feubli 12:9e90d7a5a8c6 241 break;
joe_feubli 12:9e90d7a5a8c6 242 }
joe_feubli 12:9e90d7a5a8c6 243
joe_feubli 6:3eaa420dc2f5 244 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 12:9e90d7a5a8c6 245 pc.printf("Homing beendet\n");
joe_feubli 13:cc43e546f310 246 node5_grundposition = false;
joe_feubli 13:cc43e546f310 247 Power_ein = true;
joe_feubli 7:44f2f9fd2eeb 248 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 249 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 250 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 251 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 252 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 253 MyEpos5.Power(0);
joe_feubli 11:6e5ebe9aa84b 254 current_step = 2;//BEREIT;
joe_feubli 2:0f4e77a22fd9 255 }
joe_feubli 10:1c42bbd596dc 256
joe_feubli 8:98d4028cdea6 257
joe_feubli 6:3eaa420dc2f5 258 led1 = 0;
joe_feubli 2:0f4e77a22fd9 259 break;
joe_feubli 2:0f4e77a22fd9 260
joe_feubli 2:0f4e77a22fd9 261
joe_feubli 2:0f4e77a22fd9 262 case 2://BEREIT:
joe_feubli 6:3eaa420dc2f5 263 led2 = 1;
joe_feubli 8:98d4028cdea6 264 MyEpos5.SetDigOut(4,0);
joe_feubli 6:3eaa420dc2f5 265 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 8:98d4028cdea6 266 MyEpos5.SetDigOut(4,1);
joe_feubli 7:44f2f9fd2eeb 267 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 268 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 269 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 270 MyEpos3.Power(1);
joe_feubli 11:6e5ebe9aa84b 271 MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 272 MyEpos5.Power(1);
joe_feubli 14:bee23de5dff0 273 MyEpos6.Power(1);
joe_feubli 14:bee23de5dff0 274 MyEpos7.Power(1);
joe_feubli 12:9e90d7a5a8c6 275 einfahren = false;
joe_feubli 13:cc43e546f310 276 ausfahren_step = 1;
joe_feubli 13:cc43e546f310 277 current_step = 3;
joe_feubli 11:6e5ebe9aa84b 278 wait(0.5);
joe_feubli 6:3eaa420dc2f5 279 }
joe_feubli 14:bee23de5dff0 280 wait(0.1);
joe_feubli 14:bee23de5dff0 281 /********TEST********/ /*
joe_feubli 14:bee23de5dff0 282 MyEpos5.SetDigOut(4,1);
joe_feubli 11:6e5ebe9aa84b 283 wait_ms(10);
joe_feubli 14:bee23de5dff0 284 MyEpos1.Power(1);
joe_feubli 14:bee23de5dff0 285 MyEpos2.Power(1);
joe_feubli 14:bee23de5dff0 286 MyEpos3.Power(1);
joe_feubli 14:bee23de5dff0 287 MyEpos4.Power(1);
joe_feubli 14:bee23de5dff0 288 MyEpos5.Power(1);
joe_feubli 14:bee23de5dff0 289 MyEpos6.Power(1);
joe_feubli 14:bee23de5dff0 290 MyEpos7.Power(1);
joe_feubli 14:bee23de5dff0 291 einfahren = false;
joe_feubli 14:bee23de5dff0 292 ausfahren_step = 1;
joe_feubli 14:bee23de5dff0 293 current_step = 3;
joe_feubli 14:bee23de5dff0 294 */ /********TEST*******/
joe_feubli 9:039318b9096e 295 led2 = 0;
joe_feubli 2:0f4e77a22fd9 296 break;
joe_feubli 13:cc43e546f310 297
joe_feubli 13:cc43e546f310 298 case 3://ABLAUF
joe_feubli 12:9e90d7a5a8c6 299 // pc.printf("Läuft...\n");
joe_feubli 7:44f2f9fd2eeb 300 led3 = 1;
joe_feubli 6:3eaa420dc2f5 301 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 302 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 303 case 1:
joe_feubli 13:cc43e546f310 304 MyEpos5.MoveAbsolute(-150000,800*speed,100*speed,100*speed); //Kopf
joe_feubli 6:3eaa420dc2f5 305 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 306 wait(1);
joe_feubli 6:3eaa420dc2f5 307 break;
joe_feubli 6:3eaa420dc2f5 308
joe_feubli 6:3eaa420dc2f5 309 case 2:
joe_feubli 6:3eaa420dc2f5 310 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 311 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 312 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 313 MyEpos2.MoveAbsolute(310000,500*speed,100*speed,100*speed); //Panel Innen Links
joe_feubli 13:cc43e546f310 314 MyEpos4.MoveAbsolute(-310000,500*speed,100*speed,100*speed); //Panel Innen Rechts
joe_feubli 6:3eaa420dc2f5 315 ausfahren_step = 3;
joe_feubli 10:1c42bbd596dc 316 wait(1);
joe_feubli 6:3eaa420dc2f5 317 }
joe_feubli 14:bee23de5dff0 318 wait(0.1);
joe_feubli 6:3eaa420dc2f5 319 break;
joe_feubli 6:3eaa420dc2f5 320
joe_feubli 6:3eaa420dc2f5 321 case 3:
joe_feubli 13:cc43e546f310 322 if (MyEpos2.GetActualPos() > 280000 and MyEpos4.GetActualPos() < -280000){
joe_feubli 13:cc43e546f310 323 MyEpos1.MoveAbsolute(318000,500*speed,100*speed,100*speed); //Panel Aussen Links
joe_feubli 13:cc43e546f310 324 MyEpos3.MoveAbsolute(-318000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 325
joe_feubli 6:3eaa420dc2f5 326 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 327 wait(1);
joe_feubli 6:3eaa420dc2f5 328 }
joe_feubli 14:bee23de5dff0 329 wait(0.1);
joe_feubli 6:3eaa420dc2f5 330 break;
joe_feubli 6:3eaa420dc2f5 331
joe_feubli 6:3eaa420dc2f5 332 case 4:
joe_feubli 6:3eaa420dc2f5 333 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 14:bee23de5dff0 334 MyEpos6.MoveAbsolute(-76000,500*speed,100*speed,100*speed); //Bohrgestell
joe_feubli 14:bee23de5dff0 335 ausfahren_step = 5;
joe_feubli 14:bee23de5dff0 336 wait(1);
joe_feubli 14:bee23de5dff0 337 }
joe_feubli 14:bee23de5dff0 338 wait(0.1);
joe_feubli 14:bee23de5dff0 339 break;
joe_feubli 14:bee23de5dff0 340
joe_feubli 14:bee23de5dff0 341
joe_feubli 14:bee23de5dff0 342 case 5:
joe_feubli 14:bee23de5dff0 343 if (MyEpos6.TargetReached()==1){
joe_feubli 14:bee23de5dff0 344 MyEpos7.MoveAbsolute(700000,2000*speed,500*speed,500*speed); //Bohrer
joe_feubli 14:bee23de5dff0 345 ausfahren_step = 6;
joe_feubli 14:bee23de5dff0 346 wait(1);
joe_feubli 14:bee23de5dff0 347 }
joe_feubli 14:bee23de5dff0 348 wait(0.1);
joe_feubli 14:bee23de5dff0 349 break;
joe_feubli 14:bee23de5dff0 350
joe_feubli 14:bee23de5dff0 351
joe_feubli 14:bee23de5dff0 352 case 6:
joe_feubli 14:bee23de5dff0 353 if (MyEpos7.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 354 einfahren = true;
joe_feubli 6:3eaa420dc2f5 355 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 356 wait(1);
joe_feubli 6:3eaa420dc2f5 357 }
joe_feubli 14:bee23de5dff0 358 wait(0.1);
joe_feubli 14:bee23de5dff0 359 // while(buffering == 0){//für Fotograf
joe_feubli 14:bee23de5dff0 360 // }
joe_feubli 6:3eaa420dc2f5 361 break;
joe_feubli 14:bee23de5dff0 362
joe_feubli 14:bee23de5dff0 363 /*
joe_feubli 14:bee23de5dff0 364 case 4:
joe_feubli 14:bee23de5dff0 365 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 14:bee23de5dff0 366 einfahren = true;
joe_feubli 14:bee23de5dff0 367 einfahren_step = 3;
joe_feubli 14:bee23de5dff0 368 wait(1);
joe_feubli 14:bee23de5dff0 369 }
joe_feubli 14:bee23de5dff0 370 wait(0.1);
joe_feubli 14:bee23de5dff0 371 break;
joe_feubli 14:bee23de5dff0 372 */
joe_feubli 13:cc43e546f310 373
joe_feubli 13:cc43e546f310 374
joe_feubli 12:9e90d7a5a8c6 375
joe_feubli 12:9e90d7a5a8c6 376 }//switch (ausfahren_step)
joe_feubli 12:9e90d7a5a8c6 377 }//if (einfahren == false)
joe_feubli 6:3eaa420dc2f5 378
joe_feubli 6:3eaa420dc2f5 379 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 380 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 381 switch (einfahren_step){
joe_feubli 14:bee23de5dff0 382
joe_feubli 6:3eaa420dc2f5 383 case 1:
joe_feubli 14:bee23de5dff0 384 MyEpos7.MoveAbsolute(0,2000*speed,500*speed,500*speed); //Bohrer
joe_feubli 6:3eaa420dc2f5 385 einfahren_step = 2;
joe_feubli 12:9e90d7a5a8c6 386 wait(0.1);
joe_feubli 6:3eaa420dc2f5 387 break;
joe_feubli 14:bee23de5dff0 388
joe_feubli 14:bee23de5dff0 389 case 2:
joe_feubli 14:bee23de5dff0 390 if (MyEpos7.TargetReached()==1){
joe_feubli 14:bee23de5dff0 391 MyEpos6.MoveAbsolute(0,500*speed,100*speed,100*speed); //Bohrgestell
joe_feubli 14:bee23de5dff0 392 einfahren_step = 3;
joe_feubli 14:bee23de5dff0 393 wait(1);
joe_feubli 14:bee23de5dff0 394 }
joe_feubli 14:bee23de5dff0 395 wait(0.1);
joe_feubli 14:bee23de5dff0 396 break;
joe_feubli 6:3eaa420dc2f5 397
joe_feubli 14:bee23de5dff0 398 case 3:
joe_feubli 14:bee23de5dff0 399 // if (MyEpos7.TargetReached()==1){
joe_feubli 14:bee23de5dff0 400 MyEpos1.MoveAbsolute(10000,500*speed,100*speed,100*speed); //Panel Aussen Links
joe_feubli 14:bee23de5dff0 401 MyEpos3.MoveAbsolute(-10000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
joe_feubli 14:bee23de5dff0 402 einfahren_step = 4;
joe_feubli 14:bee23de5dff0 403 // wait(1);
joe_feubli 14:bee23de5dff0 404 // }
joe_feubli 14:bee23de5dff0 405 wait(0.1);
joe_feubli 14:bee23de5dff0 406 break;
joe_feubli 14:bee23de5dff0 407
joe_feubli 14:bee23de5dff0 408 case 4:
joe_feubli 13:cc43e546f310 409 if (MyEpos1.GetActualPos() < 20000 and MyEpos3.GetActualPos() > -20000){
joe_feubli 8:98d4028cdea6 410 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 411 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 412 MyEpos2.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Links
joe_feubli 13:cc43e546f310 413 MyEpos4.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Rechts
joe_feubli 14:bee23de5dff0 414 einfahren_step = 5;
joe_feubli 12:9e90d7a5a8c6 415 wait(0.1);
joe_feubli 6:3eaa420dc2f5 416 }
joe_feubli 14:bee23de5dff0 417 wait(0.1);
joe_feubli 6:3eaa420dc2f5 418 break;
joe_feubli 6:3eaa420dc2f5 419
joe_feubli 14:bee23de5dff0 420 case 5:
joe_feubli 11:6e5ebe9aa84b 421 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
joe_feubli 13:cc43e546f310 422 MyEpos5.MoveAbsolute(1000,500*speed,100*speed,100*speed); //Kopf
joe_feubli 6:3eaa420dc2f5 423 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 424 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 425 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 426 MyEpos4.Power(0);
joe_feubli 14:bee23de5dff0 427 einfahren_step = 6;
joe_feubli 12:9e90d7a5a8c6 428 wait(0.1);
joe_feubli 6:3eaa420dc2f5 429 }
joe_feubli 14:bee23de5dff0 430 wait(0.1);
joe_feubli 6:3eaa420dc2f5 431 break;
joe_feubli 6:3eaa420dc2f5 432
joe_feubli 14:bee23de5dff0 433 case 6:
joe_feubli 6:3eaa420dc2f5 434 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 435 einfahren = false;
joe_feubli 6:3eaa420dc2f5 436 current_step = 2;
joe_feubli 6:3eaa420dc2f5 437 wait(1);
joe_feubli 6:3eaa420dc2f5 438 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 439 }
joe_feubli 14:bee23de5dff0 440 wait(0.1);
joe_feubli 10:1c42bbd596dc 441 break;
joe_feubli 10:1c42bbd596dc 442
joe_feubli 12:9e90d7a5a8c6 443 }//switch(einfahren_step)
joe_feubli 12:9e90d7a5a8c6 444 }//if einfahren == true)
joe_feubli 7:44f2f9fd2eeb 445 led3 = 0;
joe_feubli 13:cc43e546f310 446 break;
joe_feubli 13:cc43e546f310 447
joe_feubli 13:cc43e546f310 448 case 5://USV:
joe_feubli 13:cc43e546f310 449 pc.printf("USV\n");
joe_feubli 13:cc43e546f310 450 led4 = 1;
joe_feubli 13:cc43e546f310 451 MyEpos5.SetDigOut(4,1);
joe_feubli 13:cc43e546f310 452 switch (usv_step){
joe_feubli 13:cc43e546f310 453 case 0:
joe_feubli 14:bee23de5dff0 454 pc.printf("USV 0\n");
joe_feubli 14:bee23de5dff0 455 wait(0.1);
joe_feubli 13:cc43e546f310 456 MyEpos1.Power(1);
joe_feubli 13:cc43e546f310 457 MyEpos2.Power(1);
joe_feubli 13:cc43e546f310 458 MyEpos3.Power(1);
joe_feubli 13:cc43e546f310 459 MyEpos4.Power(1);
joe_feubli 13:cc43e546f310 460 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 461 MyEpos6.Power(1);
joe_feubli 13:cc43e546f310 462 MyEpos7.Power(1);
joe_feubli 13:cc43e546f310 463 usv_step = 1;
joe_feubli 14:bee23de5dff0 464 wait(0.1);
joe_feubli 13:cc43e546f310 465 break;
joe_feubli 13:cc43e546f310 466
joe_feubli 13:cc43e546f310 467 case 1:
joe_feubli 14:bee23de5dff0 468 pc.printf("USV 1\n");
joe_feubli 13:cc43e546f310 469 MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
joe_feubli 13:cc43e546f310 470 MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
joe_feubli 14:bee23de5dff0 471 MyEpos7.MoveAbsolute(0,2000,500,500); //Bohrer
joe_feubli 13:cc43e546f310 472 wait(0.1);
joe_feubli 13:cc43e546f310 473
joe_feubli 14:bee23de5dff0 474 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1 and MyEpos7.TargetReached()==1){
joe_feubli 13:cc43e546f310 475 usv_step = 2;
joe_feubli 13:cc43e546f310 476 wait(0.1);
joe_feubli 13:cc43e546f310 477 }
joe_feubli 13:cc43e546f310 478 break;
joe_feubli 13:cc43e546f310 479
joe_feubli 13:cc43e546f310 480 case 2:
joe_feubli 14:bee23de5dff0 481 pc.printf("USV 2\n");
joe_feubli 13:cc43e546f310 482 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 483 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 484
joe_feubli 13:cc43e546f310 485 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 13:cc43e546f310 486 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 14:bee23de5dff0 487
joe_feubli 14:bee23de5dff0 488 MyEpos6.MoveAbsolute(0,500,100,100); //Bohrgestell
joe_feubli 13:cc43e546f310 489 wait(0.1);
joe_feubli 13:cc43e546f310 490
joe_feubli 13:cc43e546f310 491 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
joe_feubli 13:cc43e546f310 492 usv_step = 3;
joe_feubli 13:cc43e546f310 493 wait(0.1);
joe_feubli 13:cc43e546f310 494 }
joe_feubli 13:cc43e546f310 495 break;
joe_feubli 13:cc43e546f310 496
joe_feubli 13:cc43e546f310 497 case 3:
joe_feubli 14:bee23de5dff0 498 pc.printf("USV 3\n");
joe_feubli 14:bee23de5dff0 499 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 14:bee23de5dff0 500 wait(0.1);
joe_feubli 14:bee23de5dff0 501 if (MyEpos5.TargetReached()==1){
joe_feubli 14:bee23de5dff0 502 usv_step = 4;
joe_feubli 13:cc43e546f310 503 wait(0.1);
joe_feubli 14:bee23de5dff0 504 }
joe_feubli 13:cc43e546f310 505 break;
joe_feubli 13:cc43e546f310 506
joe_feubli 13:cc43e546f310 507 case 4:
joe_feubli 14:bee23de5dff0 508 pc.printf("USV 4\n");
joe_feubli 13:cc43e546f310 509 MyEpos1.Power(0);
joe_feubli 13:cc43e546f310 510 MyEpos2.Power(0);
joe_feubli 13:cc43e546f310 511 MyEpos3.Power(0);
joe_feubli 13:cc43e546f310 512 MyEpos4.Power(0);
joe_feubli 13:cc43e546f310 513 MyEpos5.Power(0);
joe_feubli 13:cc43e546f310 514 MyEpos6.Power(0);
joe_feubli 13:cc43e546f310 515 MyEpos7.Power(0);
joe_feubli 13:cc43e546f310 516 usv_step = 5;
joe_feubli 13:cc43e546f310 517 wait(0.1);
joe_feubli 13:cc43e546f310 518 break;
joe_feubli 13:cc43e546f310 519
joe_feubli 13:cc43e546f310 520 case 5:
joe_feubli 14:bee23de5dff0 521 pc.printf("USV 5\n");
joe_feubli 13:cc43e546f310 522 if(buffering == 1){
joe_feubli 13:cc43e546f310 523 led3 = 1;
joe_feubli 13:cc43e546f310 524 } else {
joe_feubli 13:cc43e546f310 525 led3 = 0;
joe_feubli 13:cc43e546f310 526 Power_ein = true;
joe_feubli 13:cc43e546f310 527 current_step=1;
joe_feubli 13:cc43e546f310 528 usv_step=0;
joe_feubli 13:cc43e546f310 529 ausfahren_step=1;
joe_feubli 13:cc43e546f310 530 einfahren_step=1;
joe_feubli 14:bee23de5dff0 531 wait(0.1);
joe_feubli 14:bee23de5dff0 532 break;
joe_feubli 13:cc43e546f310 533 }
joe_feubli 13:cc43e546f310 534 wait(1);
joe_feubli 13:cc43e546f310 535 break;
joe_feubli 13:cc43e546f310 536 }//switch(usv_step)
joe_feubli 13:cc43e546f310 537 led4 = 0;
joe_feubli 13:cc43e546f310 538 break;
joe_feubli 13:cc43e546f310 539
joe_feubli 14:bee23de5dff0 540 case 6: //Fehler Bohrer/Bohrgestell
joe_feubli 13:cc43e546f310 541 pc.printf("Fehler RESET Bohrer\n");
joe_feubli 14:bee23de5dff0 542 MyEpos7.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 543 if (MyEpos7.TargetReached() == 1){
joe_feubli 14:bee23de5dff0 544 MyEpos6.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 545 if(MyEpos6.TargetReached() == 1){
joe_feubli 14:bee23de5dff0 546 MyEpos5.Reset();
joe_feubli 14:bee23de5dff0 547 MyEpos5.Power(1);
joe_feubli 14:bee23de5dff0 548 MyEpos6.Reset();
joe_feubli 14:bee23de5dff0 549 MyEpos6.Power(1);
joe_feubli 14:bee23de5dff0 550 current_step = 1;
joe_feubli 14:bee23de5dff0 551 }
joe_feubli 14:bee23de5dff0 552 }
joe_feubli 13:cc43e546f310 553 //node1_homed = false;
joe_feubli 13:cc43e546f310 554 //node2_homed = false;
joe_feubli 14:bee23de5dff0 555 //current_step = 1;
ODEM 1:c53aafa72b36 556 break;
joe_feubli 12:9e90d7a5a8c6 557
joe_feubli 12:9e90d7a5a8c6 558 case 7:
joe_feubli 13:cc43e546f310 559 pc.printf("Fehler RESET\n");
joe_feubli 12:9e90d7a5a8c6 560 MyEpos1.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 561 MyEpos2.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 562 MyEpos3.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 563 MyEpos4.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 564 MyEpos5.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 565 wait(1);
joe_feubli 12:9e90d7a5a8c6 566 MyEpos1.Reset();
joe_feubli 12:9e90d7a5a8c6 567 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 568 MyEpos2.Reset();
joe_feubli 12:9e90d7a5a8c6 569 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 570 MyEpos3.Reset();
joe_feubli 12:9e90d7a5a8c6 571 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 572 MyEpos4.Reset();
joe_feubli 12:9e90d7a5a8c6 573 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 574 MyEpos5.Reset();
joe_feubli 12:9e90d7a5a8c6 575 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 576 epos1_homing = false;
joe_feubli 13:cc43e546f310 577 epos3_homing = false;
joe_feubli 12:9e90d7a5a8c6 578 node1_homed = false;
joe_feubli 12:9e90d7a5a8c6 579 node2_homed = false;
joe_feubli 12:9e90d7a5a8c6 580 node3_homed = false;
joe_feubli 12:9e90d7a5a8c6 581 node4_homed = false;
joe_feubli 12:9e90d7a5a8c6 582 node5_homed = false;
joe_feubli 13:cc43e546f310 583 Power_ein = true;
joe_feubli 13:cc43e546f310 584 if(MyEpos2.GetActualPos() < 100000 or MyEpos4.GetActualPos() > -100000){
joe_feubli 14:bee23de5dff0 585 MyEpos1.Homing();
joe_feubli 14:bee23de5dff0 586 MyEpos3.Homing();
joe_feubli 14:bee23de5dff0 587 MyEpos1.MoveAbsolute(0,500,100,100);
joe_feubli 14:bee23de5dff0 588 MyEpos3.MoveAbsolute(0,500,100,100);
joe_feubli 13:cc43e546f310 589 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
joe_feubli 13:cc43e546f310 590 MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
joe_feubli 13:cc43e546f310 591 while (MyEpos2.TargetReached() != 1 or MyEpos4.TargetReached() != 1){
joe_feubli 13:cc43e546f310 592 if(MyEpos2.GetError() == 1 or MyEpos4.GetError() == 1){
joe_feubli 14:bee23de5dff0 593 //wait(0.1);
joe_feubli 13:cc43e546f310 594 break;
joe_feubli 13:cc43e546f310 595 }
joe_feubli 14:bee23de5dff0 596 //wait(0.1);
joe_feubli 13:cc43e546f310 597 }
joe_feubli 13:cc43e546f310 598 }
joe_feubli 14:bee23de5dff0 599 current_step = 6;
joe_feubli 12:9e90d7a5a8c6 600 wait(0.1);
joe_feubli 13:cc43e546f310 601
joe_feubli 12:9e90d7a5a8c6 602 break;
joe_feubli 12:9e90d7a5a8c6 603 }//switch (current_step)
joe_feubli 2:0f4e77a22fd9 604 }//while
joe_feubli 10:1c42bbd596dc 605 }//main