mmotor / Mbed 2 deprecated MarsRover_ExoMars

Programm mit Ablauf

Dependencies:   EPOS2 mbed-rtos mbed

Fork of Projekt_Kugelbahn by mmotor

main.cpp@13:cc43e546f310, 2016-10-19 (annotated)

Committer:
joe_feubli
Date:
Wed Oct 19 10:02:02 2016 +0000
Revision:
13:cc43e546f310
Parent:
12:9e90d7a5a8c6
Child:
14:bee23de5dff0
Alles Funktioniert(viele Fehlerf?lle sind ausgeschlossen) ohne 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 6:3eaa420dc2f5 30 bool node6_homed = true;
joe_feubli 6:3eaa420dc2f5 31 bool node7_homed = true;
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 13:cc43e546f310 50 float speed = 1.2;
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 13:cc43e546f310 109 ErrorState1 = MyEpos1.GetError();
joe_feubli 13:cc43e546f310 110 ErrorState2 = MyEpos2.GetError();
joe_feubli 13:cc43e546f310 111 ErrorState3 = MyEpos3.GetError();
joe_feubli 13:cc43e546f310 112 ErrorState4 = MyEpos4.GetError();
joe_feubli 13:cc43e546f310 113 ErrorState5 = MyEpos5.GetError();
joe_feubli 12:9e90d7a5a8c6 114 wait(0.1);
joe_feubli 13:cc43e546f310 115 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 116 }
joe_feubli 11:6e5ebe9aa84b 117
joe_feubli 13:cc43e546f310 118 /* if(MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0){
joe_feubli 13:cc43e546f310 119 current_step = 76; //FEHLER
joe_feubli 13:cc43e546f310 120 ErrorState6 = MyEpos6.GetError();
joe_feubli 13:cc43e546f310 121 ErrorState7 = MyEpos7.GetError();
joe_feubli 13:cc43e546f310 122 wait(0.1);
joe_feubli 13:cc43e546f310 123 pc.printf("Bohrgestell/Bohrer ErrorState: Bohrgestell=%d, Bohrer=%d\r\n", ErrorState6, ErrorState7);
joe_feubli 13:cc43e546f310 124 }
joe_feubli 13:cc43e546f310 125
joe_feubli 13:cc43e546f310 126 */ switch (current_step){
joe_feubli 2:0f4e77a22fd9 127 case 1://HOMING:
joe_feubli 12:9e90d7a5a8c6 128
joe_feubli 6:3eaa420dc2f5 129 led1 = 1;
joe_feubli 12:9e90d7a5a8c6 130 if (Power_ein == true){
joe_feubli 12:9e90d7a5a8c6 131 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 132 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 133 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 134 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 135 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 136 MyEpos6.Power(1);
joe_feubli 12:9e90d7a5a8c6 137 MyEpos7.Power(1);
joe_feubli 12:9e90d7a5a8c6 138 Power_ein = false;
joe_feubli 12:9e90d7a5a8c6 139 }
joe_feubli 6:3eaa420dc2f5 140
joe_feubli 6:3eaa420dc2f5 141 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 142
joe_feubli 12:9e90d7a5a8c6 143
joe_feubli 7:44f2f9fd2eeb 144 if (node5_homed == false){
joe_feubli 12:9e90d7a5a8c6 145 pc.printf("Homing5\n");
joe_feubli 7:44f2f9fd2eeb 146 MyEpos5.Homing(); //Kopf
joe_feubli 12:9e90d7a5a8c6 147 //wait(5);
joe_feubli 12:9e90d7a5a8c6 148 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 149 pc.printf("Fahren auf -60000\n");
joe_feubli 12:9e90d7a5a8c6 150 while(MyEpos5.TargetReached()!=1){
joe_feubli 13:cc43e546f310 151 if(MyEpos5.GetError() == 1){
joe_feubli 13:cc43e546f310 152 break;
joe_feubli 13:cc43e546f310 153 }
joe_feubli 12:9e90d7a5a8c6 154 }
joe_feubli 12:9e90d7a5a8c6 155 pc.printf("Target reached\n");
joe_feubli 7:44f2f9fd2eeb 156 node5_homed = true;
joe_feubli 7:44f2f9fd2eeb 157 break;
joe_feubli 7:44f2f9fd2eeb 158 }
joe_feubli 12:9e90d7a5a8c6 159
joe_feubli 6:3eaa420dc2f5 160 if (node1_homed == false){
joe_feubli 13:cc43e546f310 161 if(epos1_homing == false){
joe_feubli 13:cc43e546f310 162 MyEpos1.MoveVelocity(-400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
joe_feubli 13:cc43e546f310 163 epos1_homing = true;
joe_feubli 13:cc43e546f310 164 }
joe_feubli 13:cc43e546f310 165 /* if(MyEpos1.GetError() != 0){
joe_feubli 13:cc43e546f310 166 current_step = 7;
joe_feubli 13:cc43e546f310 167 break;
joe_feubli 13:cc43e546f310 168 }
joe_feubli 13:cc43e546f310 169 */ if(MyEpos1.GetDigIn(4) == 1){
joe_feubli 13:cc43e546f310 170 pc.printf("Homing1\n");
joe_feubli 13:cc43e546f310 171 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 13:cc43e546f310 172 node1_homed = true;
joe_feubli 13:cc43e546f310 173 break;
joe_feubli 13:cc43e546f310 174 }
joe_feubli 6:3eaa420dc2f5 175 break;
joe_feubli 6:3eaa420dc2f5 176 }
joe_feubli 12:9e90d7a5a8c6 177
joe_feubli 6:3eaa420dc2f5 178
joe_feubli 6:3eaa420dc2f5 179 if (node3_homed == false){
joe_feubli 13:cc43e546f310 180 if (epos3_homing == false){
joe_feubli 13:cc43e546f310 181 MyEpos3.MoveVelocity(400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
joe_feubli 13:cc43e546f310 182 epos3_homing = true;
joe_feubli 13:cc43e546f310 183 }
joe_feubli 13:cc43e546f310 184 /* if(MyEpos3.GetError() != 0){
joe_feubli 13:cc43e546f310 185 current_step = 7;
joe_feubli 13:cc43e546f310 186 break;
joe_feubli 13:cc43e546f310 187 }
joe_feubli 13:cc43e546f310 188 */ if(MyEpos3.GetDigIn(4) == 1){
joe_feubli 13:cc43e546f310 189 pc.printf("Homing3\n");
joe_feubli 13:cc43e546f310 190 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 13:cc43e546f310 191 node3_homed = true;
joe_feubli 13:cc43e546f310 192 }
joe_feubli 6:3eaa420dc2f5 193 break;
joe_feubli 6:3eaa420dc2f5 194 }
joe_feubli 12:9e90d7a5a8c6 195
joe_feubli 6:3eaa420dc2f5 196
joe_feubli 6:3eaa420dc2f5 197 if (node7_homed == false){
joe_feubli 12:9e90d7a5a8c6 198 pc.printf("Homing7\n");
joe_feubli 6:3eaa420dc2f5 199 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 200 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 201 break;
joe_feubli 6:3eaa420dc2f5 202 }
joe_feubli 6:3eaa420dc2f5 203
joe_feubli 13:cc43e546f310 204 if (node1_homed == true and node2_homed == false){
joe_feubli 13:cc43e546f310 205 if (node5_grundposition == false){
joe_feubli 13:cc43e546f310 206 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 13:cc43e546f310 207 }
joe_feubli 12:9e90d7a5a8c6 208 pc.printf("Homing2\n");
joe_feubli 6:3eaa420dc2f5 209 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 6:3eaa420dc2f5 210 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 211 break;
joe_feubli 2:0f4e77a22fd9 212 }
joe_feubli 6:3eaa420dc2f5 213
joe_feubli 6:3eaa420dc2f5 214 if (node3_homed == true and node4_homed == false){
joe_feubli 12:9e90d7a5a8c6 215 pc.printf("Homing4\n");
joe_feubli 6:3eaa420dc2f5 216 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 217 node4_homed = true;
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 13:cc43e546f310 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 12:9e90d7a5a8c6 263 //pc.printf("Bereit\n");
joe_feubli 6:3eaa420dc2f5 264 led2 = 1;
joe_feubli 8:98d4028cdea6 265 MyEpos5.SetDigOut(4,0);
joe_feubli 6:3eaa420dc2f5 266 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 8:98d4028cdea6 267 MyEpos5.SetDigOut(4,1);
joe_feubli 7:44f2f9fd2eeb 268 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 269 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 270 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 271 MyEpos3.Power(1);
joe_feubli 11:6e5ebe9aa84b 272 MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 273 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 274 einfahren = false;
joe_feubli 13:cc43e546f310 275 ausfahren_step = 1;
joe_feubli 13:cc43e546f310 276 current_step = 3;
joe_feubli 11:6e5ebe9aa84b 277 wait(0.5);
joe_feubli 6:3eaa420dc2f5 278 }
joe_feubli 11:6e5ebe9aa84b 279 wait_ms(10);
joe_feubli 9:039318b9096e 280 led2 = 0;
joe_feubli 2:0f4e77a22fd9 281 break;
joe_feubli 13:cc43e546f310 282
joe_feubli 13:cc43e546f310 283 case 3://ABLAUF
joe_feubli 12:9e90d7a5a8c6 284 // pc.printf("Läuft...\n");
joe_feubli 7:44f2f9fd2eeb 285 led3 = 1;
joe_feubli 6:3eaa420dc2f5 286 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 287 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 288 case 1:
joe_feubli 13:cc43e546f310 289 MyEpos5.MoveAbsolute(-150000,800*speed,100*speed,100*speed); //Kopf
joe_feubli 6:3eaa420dc2f5 290 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 291 wait(1);
joe_feubli 6:3eaa420dc2f5 292 break;
joe_feubli 6:3eaa420dc2f5 293
joe_feubli 6:3eaa420dc2f5 294 case 2:
joe_feubli 6:3eaa420dc2f5 295 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 296 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 297 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 298 MyEpos2.MoveAbsolute(310000,500*speed,100*speed,100*speed); //Panel Innen Links
joe_feubli 13:cc43e546f310 299 MyEpos4.MoveAbsolute(-310000,500*speed,100*speed,100*speed); //Panel Innen Rechts
joe_feubli 6:3eaa420dc2f5 300 ausfahren_step = 3;
joe_feubli 10:1c42bbd596dc 301 wait(1);
joe_feubli 6:3eaa420dc2f5 302 }
joe_feubli 6:3eaa420dc2f5 303 break;
joe_feubli 6:3eaa420dc2f5 304
joe_feubli 6:3eaa420dc2f5 305 case 3:
joe_feubli 13:cc43e546f310 306 if (MyEpos2.GetActualPos() > 280000 and MyEpos4.GetActualPos() < -280000){
joe_feubli 13:cc43e546f310 307 MyEpos1.MoveAbsolute(318000,500*speed,100*speed,100*speed); //Panel Aussen Links
joe_feubli 13:cc43e546f310 308 MyEpos3.MoveAbsolute(-318000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 309
joe_feubli 6:3eaa420dc2f5 310 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 311 wait(1);
joe_feubli 6:3eaa420dc2f5 312 }
joe_feubli 6:3eaa420dc2f5 313 break;
joe_feubli 6:3eaa420dc2f5 314
joe_feubli 6:3eaa420dc2f5 315 case 4:
joe_feubli 6:3eaa420dc2f5 316 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 317 einfahren = true;
joe_feubli 6:3eaa420dc2f5 318 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 319 wait(1);
joe_feubli 6:3eaa420dc2f5 320 }
joe_feubli 6:3eaa420dc2f5 321 break;
joe_feubli 13:cc43e546f310 322
joe_feubli 13:cc43e546f310 323
joe_feubli 12:9e90d7a5a8c6 324
joe_feubli 12:9e90d7a5a8c6 325 }//switch (ausfahren_step)
joe_feubli 12:9e90d7a5a8c6 326 }//if (einfahren == false)
joe_feubli 6:3eaa420dc2f5 327
joe_feubli 6:3eaa420dc2f5 328 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 329 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 330 switch (einfahren_step){
joe_feubli 6:3eaa420dc2f5 331 case 1:
joe_feubli 13:cc43e546f310 332 MyEpos1.MoveAbsolute(4000,500*speed,100*speed,100*speed); //Panel Aussen Links
joe_feubli 13:cc43e546f310 333 MyEpos3.MoveAbsolute(-4000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 334 einfahren_step = 2;
joe_feubli 12:9e90d7a5a8c6 335 wait(0.1);
joe_feubli 6:3eaa420dc2f5 336 break;
joe_feubli 6:3eaa420dc2f5 337
joe_feubli 6:3eaa420dc2f5 338 case 2:
joe_feubli 13:cc43e546f310 339 if (MyEpos1.GetActualPos() < 20000 and MyEpos3.GetActualPos() > -20000){
joe_feubli 8:98d4028cdea6 340 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 341 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 342 MyEpos2.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Links
joe_feubli 13:cc43e546f310 343 MyEpos4.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Rechts
joe_feubli 6:3eaa420dc2f5 344 einfahren_step = 3;
joe_feubli 12:9e90d7a5a8c6 345 wait(0.1);
joe_feubli 6:3eaa420dc2f5 346 }
joe_feubli 6:3eaa420dc2f5 347 break;
joe_feubli 6:3eaa420dc2f5 348
joe_feubli 6:3eaa420dc2f5 349 case 3:
joe_feubli 11:6e5ebe9aa84b 350 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
joe_feubli 13:cc43e546f310 351 MyEpos5.MoveAbsolute(1000,500*speed,100*speed,100*speed); //Kopf
joe_feubli 6:3eaa420dc2f5 352 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 353 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 354 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 355 MyEpos4.Power(0);
joe_feubli 6:3eaa420dc2f5 356 einfahren_step = 4;
joe_feubli 12:9e90d7a5a8c6 357 wait(0.1);
joe_feubli 6:3eaa420dc2f5 358 }
joe_feubli 6:3eaa420dc2f5 359 break;
joe_feubli 6:3eaa420dc2f5 360
joe_feubli 6:3eaa420dc2f5 361 case 4:
joe_feubli 6:3eaa420dc2f5 362 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 363 einfahren = false;
joe_feubli 6:3eaa420dc2f5 364 current_step = 2;
joe_feubli 6:3eaa420dc2f5 365 wait(1);
joe_feubli 6:3eaa420dc2f5 366 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 367 }
joe_feubli 10:1c42bbd596dc 368 break;
joe_feubli 10:1c42bbd596dc 369
joe_feubli 12:9e90d7a5a8c6 370 }//switch(einfahren_step)
joe_feubli 12:9e90d7a5a8c6 371 }//if einfahren == true)
joe_feubli 7:44f2f9fd2eeb 372 led3 = 0;
joe_feubli 13:cc43e546f310 373 break;
joe_feubli 13:cc43e546f310 374
joe_feubli 13:cc43e546f310 375 case 5://USV:
joe_feubli 13:cc43e546f310 376 pc.printf("USV\n");
joe_feubli 13:cc43e546f310 377 led4 = 1;
joe_feubli 13:cc43e546f310 378 MyEpos5.SetDigOut(4,1);
joe_feubli 13:cc43e546f310 379 switch (usv_step){
joe_feubli 13:cc43e546f310 380 case 0:
joe_feubli 13:cc43e546f310 381 MyEpos1.Power(1);
joe_feubli 13:cc43e546f310 382 MyEpos2.Power(1);
joe_feubli 13:cc43e546f310 383 MyEpos3.Power(1);
joe_feubli 13:cc43e546f310 384 MyEpos4.Power(1);
joe_feubli 13:cc43e546f310 385 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 386 MyEpos6.Power(1);
joe_feubli 13:cc43e546f310 387 MyEpos7.Power(1);
joe_feubli 13:cc43e546f310 388 usv_step = 1;
joe_feubli 13:cc43e546f310 389 break;
joe_feubli 13:cc43e546f310 390
joe_feubli 13:cc43e546f310 391 case 1:
joe_feubli 13:cc43e546f310 392 MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
joe_feubli 13:cc43e546f310 393 MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
joe_feubli 13:cc43e546f310 394 wait(0.1);
joe_feubli 13:cc43e546f310 395
joe_feubli 13:cc43e546f310 396 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 13:cc43e546f310 397 usv_step = 2;
joe_feubli 13:cc43e546f310 398 wait(0.1);
joe_feubli 13:cc43e546f310 399 }
joe_feubli 13:cc43e546f310 400 break;
joe_feubli 13:cc43e546f310 401
joe_feubli 13:cc43e546f310 402 case 2:
joe_feubli 13:cc43e546f310 403
joe_feubli 13:cc43e546f310 404 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 405 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 406
joe_feubli 13:cc43e546f310 407 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 13:cc43e546f310 408 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 13:cc43e546f310 409 wait(0.1);
joe_feubli 13:cc43e546f310 410
joe_feubli 13:cc43e546f310 411 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
joe_feubli 13:cc43e546f310 412 usv_step = 3;
joe_feubli 13:cc43e546f310 413 wait(0.1);
joe_feubli 13:cc43e546f310 414 }
joe_feubli 13:cc43e546f310 415 break;
joe_feubli 13:cc43e546f310 416
joe_feubli 13:cc43e546f310 417 case 3:
joe_feubli 13:cc43e546f310 418 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 13:cc43e546f310 419 wait(0.1);
joe_feubli 13:cc43e546f310 420 if (MyEpos5.TargetReached()==1){
joe_feubli 13:cc43e546f310 421 usv_step = 4;
joe_feubli 13:cc43e546f310 422 wait(0.1);
joe_feubli 13:cc43e546f310 423 }
joe_feubli 13:cc43e546f310 424 break;
joe_feubli 13:cc43e546f310 425
joe_feubli 13:cc43e546f310 426 case 4:
joe_feubli 13:cc43e546f310 427 MyEpos1.Power(0);
joe_feubli 13:cc43e546f310 428 MyEpos2.Power(0);
joe_feubli 13:cc43e546f310 429 MyEpos3.Power(0);
joe_feubli 13:cc43e546f310 430 MyEpos4.Power(0);
joe_feubli 13:cc43e546f310 431 MyEpos5.Power(0);
joe_feubli 13:cc43e546f310 432 MyEpos6.Power(0);
joe_feubli 13:cc43e546f310 433 MyEpos7.Power(0);
joe_feubli 13:cc43e546f310 434 usv_step = 5;
joe_feubli 13:cc43e546f310 435 wait(0.1);
joe_feubli 13:cc43e546f310 436 break;
joe_feubli 13:cc43e546f310 437
joe_feubli 13:cc43e546f310 438 case 5:
joe_feubli 13:cc43e546f310 439 if(buffering == 1){
joe_feubli 13:cc43e546f310 440 led3 = 1;
joe_feubli 13:cc43e546f310 441 } else {
joe_feubli 13:cc43e546f310 442 led3 = 0;
joe_feubli 13:cc43e546f310 443 Power_ein = true;
joe_feubli 13:cc43e546f310 444 current_step=1;
joe_feubli 13:cc43e546f310 445 usv_step=0;
joe_feubli 13:cc43e546f310 446 ausfahren_step=1;
joe_feubli 13:cc43e546f310 447 einfahren_step=1;
joe_feubli 13:cc43e546f310 448 }
joe_feubli 13:cc43e546f310 449 wait(1);
joe_feubli 13:cc43e546f310 450 break;
joe_feubli 13:cc43e546f310 451 }//switch(usv_step)
joe_feubli 13:cc43e546f310 452 led4 = 0;
joe_feubli 13:cc43e546f310 453 break;
joe_feubli 13:cc43e546f310 454
joe_feubli 13:cc43e546f310 455 case 6:
joe_feubli 13:cc43e546f310 456 pc.printf("Fehler RESET Bohrer\n");
joe_feubli 13:cc43e546f310 457 MyEpos5.MoveVelocity(0, 500, 500);
joe_feubli 13:cc43e546f310 458 MyEpos6.MoveVelocity(0, 500, 500);
joe_feubli 13:cc43e546f310 459 wait(3);
joe_feubli 13:cc43e546f310 460 MyEpos5.Reset();
joe_feubli 13:cc43e546f310 461 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 462 MyEpos6.Reset();
joe_feubli 13:cc43e546f310 463 MyEpos6.Power(1);
joe_feubli 13:cc43e546f310 464 //node1_homed = false;
joe_feubli 13:cc43e546f310 465 //node2_homed = false;
joe_feubli 13:cc43e546f310 466 current_step = 3;
ODEM 1:c53aafa72b36 467 break;
joe_feubli 12:9e90d7a5a8c6 468
joe_feubli 12:9e90d7a5a8c6 469 case 7:
joe_feubli 13:cc43e546f310 470 pc.printf("Fehler RESET\n");
joe_feubli 12:9e90d7a5a8c6 471 MyEpos1.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 472 MyEpos2.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 473 MyEpos3.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 474 MyEpos4.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 475 MyEpos5.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 476 wait(3);
joe_feubli 12:9e90d7a5a8c6 477 MyEpos1.Reset();
joe_feubli 12:9e90d7a5a8c6 478 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 479 MyEpos2.Reset();
joe_feubli 12:9e90d7a5a8c6 480 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 481 MyEpos3.Reset();
joe_feubli 12:9e90d7a5a8c6 482 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 483 MyEpos4.Reset();
joe_feubli 12:9e90d7a5a8c6 484 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 485 MyEpos5.Reset();
joe_feubli 12:9e90d7a5a8c6 486 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 487 epos1_homing = false;
joe_feubli 13:cc43e546f310 488 epos3_homing = false;
joe_feubli 12:9e90d7a5a8c6 489 node1_homed = false;
joe_feubli 12:9e90d7a5a8c6 490 node2_homed = false;
joe_feubli 12:9e90d7a5a8c6 491 node3_homed = false;
joe_feubli 12:9e90d7a5a8c6 492 node4_homed = false;
joe_feubli 12:9e90d7a5a8c6 493 node5_homed = false;
joe_feubli 13:cc43e546f310 494 Power_ein = true;
joe_feubli 13:cc43e546f310 495 if(MyEpos2.GetActualPos() < 100000 or MyEpos4.GetActualPos() > -100000){
joe_feubli 13:cc43e546f310 496 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 497 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 498 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
joe_feubli 13:cc43e546f310 499 MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
joe_feubli 13:cc43e546f310 500 while (MyEpos2.TargetReached() != 1 or MyEpos4.TargetReached() != 1){
joe_feubli 13:cc43e546f310 501 if(MyEpos2.GetError() == 1 or MyEpos4.GetError() == 1){
joe_feubli 13:cc43e546f310 502 break;
joe_feubli 13:cc43e546f310 503 }
joe_feubli 13:cc43e546f310 504 }
joe_feubli 13:cc43e546f310 505 }
joe_feubli 12:9e90d7a5a8c6 506 current_step = 1;
joe_feubli 12:9e90d7a5a8c6 507 wait(0.1);
joe_feubli 13:cc43e546f310 508
joe_feubli 12:9e90d7a5a8c6 509 break;
joe_feubli 12:9e90d7a5a8c6 510 }//switch (current_step)
joe_feubli 2:0f4e77a22fd9 511 }//while
joe_feubli 10:1c42bbd596dc 512 }//main