mmotor / Mbed 2 deprecated MarsRover_ExoMars

Programm mit Ablauf

Dependencies:   EPOS2 mbed-rtos mbed

Fork of Projekt_Kugelbahn by mmotor

main.cpp@16:e5343d6c9fb2, 2016-11-07 (annotated)

Committer:
joe_feubli
Date:
Mon Nov 07 07:33:03 2016 +0000
Revision:
16:e5343d6c9fb2
Parent:
14:bee23de5dff0 
Ablauffehler behoben

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 16:e5343d6c9fb2 135
joe_feubli 16:e5343d6c9fb2 136 if(buffering == 1){
joe_feubli 16:e5343d6c9fb2 137 current_step = 5;
joe_feubli 16:e5343d6c9fb2 138 }
joe_feubli 13:cc43e546f310 139
joe_feubli 14:bee23de5dff0 140 switch (current_step){
joe_feubli 2:0f4e77a22fd9 141 case 1://HOMING:
joe_feubli 12:9e90d7a5a8c6 142
joe_feubli 6:3eaa420dc2f5 143 led1 = 1;
joe_feubli 12:9e90d7a5a8c6 144 if (Power_ein == true){
joe_feubli 12:9e90d7a5a8c6 145 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 146 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 147 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 148 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 149 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 150 MyEpos6.Power(1);
joe_feubli 12:9e90d7a5a8c6 151 MyEpos7.Power(1);
joe_feubli 12:9e90d7a5a8c6 152 Power_ein = false;
joe_feubli 12:9e90d7a5a8c6 153 }
joe_feubli 6:3eaa420dc2f5 154
joe_feubli 6:3eaa420dc2f5 155 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 156
joe_feubli 6:3eaa420dc2f5 157 if (node1_homed == false){
joe_feubli 13:cc43e546f310 158 if(epos1_homing == false){
joe_feubli 13:cc43e546f310 159 MyEpos1.MoveVelocity(-400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
joe_feubli 13:cc43e546f310 160 epos1_homing = true;
joe_feubli 13:cc43e546f310 161 }
joe_feubli 14:bee23de5dff0 162 if(MyEpos1.GetDigIn(4) == 1){
joe_feubli 13:cc43e546f310 163 pc.printf("Homing1\n");
joe_feubli 13:cc43e546f310 164 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 13:cc43e546f310 165 node1_homed = true;
joe_feubli 13:cc43e546f310 166 break;
joe_feubli 13:cc43e546f310 167 }
joe_feubli 6:3eaa420dc2f5 168 break;
joe_feubli 6:3eaa420dc2f5 169 }
joe_feubli 12:9e90d7a5a8c6 170
joe_feubli 6:3eaa420dc2f5 171
joe_feubli 6:3eaa420dc2f5 172 if (node3_homed == false){
joe_feubli 13:cc43e546f310 173 if (epos3_homing == false){
joe_feubli 13:cc43e546f310 174 MyEpos3.MoveVelocity(400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
joe_feubli 13:cc43e546f310 175 epos3_homing = true;
joe_feubli 13:cc43e546f310 176 }
joe_feubli 14:bee23de5dff0 177 if(MyEpos3.GetDigIn(4) == 1){
joe_feubli 13:cc43e546f310 178 pc.printf("Homing3\n");
joe_feubli 13:cc43e546f310 179 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 13:cc43e546f310 180 node3_homed = true;
joe_feubli 13:cc43e546f310 181 }
joe_feubli 6:3eaa420dc2f5 182 break;
joe_feubli 6:3eaa420dc2f5 183 }
joe_feubli 6:3eaa420dc2f5 184
joe_feubli 14:bee23de5dff0 185 if (node5_homed == false){
joe_feubli 14:bee23de5dff0 186 pc.printf("Homing5\n");
joe_feubli 14:bee23de5dff0 187 MyEpos5.Homing(); //Kopf
joe_feubli 14:bee23de5dff0 188 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 14:bee23de5dff0 189 pc.printf("Fahren auf -60000\n");
joe_feubli 14:bee23de5dff0 190 while(MyEpos5.TargetReached()!=1){
joe_feubli 14:bee23de5dff0 191 if(MyEpos5.GetError() == 1){
joe_feubli 14:bee23de5dff0 192 break;
joe_feubli 14:bee23de5dff0 193 }
joe_feubli 14:bee23de5dff0 194 }
joe_feubli 14:bee23de5dff0 195 pc.printf("Target reached\n");
joe_feubli 14:bee23de5dff0 196 node5_homed = true;
joe_feubli 14:bee23de5dff0 197 break;
joe_feubli 14:bee23de5dff0 198 }
joe_feubli 14:bee23de5dff0 199
joe_feubli 6:3eaa420dc2f5 200 if (node7_homed == false){
joe_feubli 12:9e90d7a5a8c6 201 pc.printf("Homing7\n");
joe_feubli 6:3eaa420dc2f5 202 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 203 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 204 break;
joe_feubli 6:3eaa420dc2f5 205 }
joe_feubli 6:3eaa420dc2f5 206
joe_feubli 13:cc43e546f310 207 if (node1_homed == true and node2_homed == false){
joe_feubli 13:cc43e546f310 208 if (node5_grundposition == false){
joe_feubli 13:cc43e546f310 209 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 13:cc43e546f310 210 }
joe_feubli 12:9e90d7a5a8c6 211 pc.printf("Homing2\n");
joe_feubli 6:3eaa420dc2f5 212 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 6:3eaa420dc2f5 213 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 214 break;
joe_feubli 2:0f4e77a22fd9 215 }
joe_feubli 6:3eaa420dc2f5 216
joe_feubli 6:3eaa420dc2f5 217 if (node3_homed == true and node4_homed == false){
joe_feubli 12:9e90d7a5a8c6 218 pc.printf("Homing4\n");
joe_feubli 6:3eaa420dc2f5 219 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 220 node4_homed = true;
joe_feubli 14:bee23de5dff0 221 MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 222 wait(0.1);
joe_feubli 6:3eaa420dc2f5 223 break;
joe_feubli 6:3eaa420dc2f5 224 }
joe_feubli 12:9e90d7a5a8c6 225
joe_feubli 12:9e90d7a5a8c6 226
joe_feubli 6:3eaa420dc2f5 227
joe_feubli 6:3eaa420dc2f5 228 if (node7_homed == true and node6_homed == false){
joe_feubli 12:9e90d7a5a8c6 229 pc.printf("Homing6\n");
joe_feubli 6:3eaa420dc2f5 230 MyEpos6.Homing();//Bohrgestell
joe_feubli 6:3eaa420dc2f5 231 node6_homed = true;
joe_feubli 6:3eaa420dc2f5 232 break;
joe_feubli 6:3eaa420dc2f5 233 }
joe_feubli 12:9e90d7a5a8c6 234
joe_feubli 13:cc43e546f310 235 if (node2_homed == true and node4_homed == true and node5_grundposition == false){
joe_feubli 14:bee23de5dff0 236 //MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 237 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 238 while(MyEpos5.TargetReached() != 1){
joe_feubli 13:cc43e546f310 239 if(MyEpos5.GetError() == 1){
joe_feubli 13:cc43e546f310 240 break;
joe_feubli 13:cc43e546f310 241 }
joe_feubli 12:9e90d7a5a8c6 242 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 243 }
joe_feubli 12:9e90d7a5a8c6 244 node5_grundposition = true;
joe_feubli 12:9e90d7a5a8c6 245 break;
joe_feubli 12:9e90d7a5a8c6 246 }
joe_feubli 12:9e90d7a5a8c6 247
joe_feubli 6:3eaa420dc2f5 248 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 249 pc.printf("Homing beendet\n");
joe_feubli 13:cc43e546f310 250 node5_grundposition = false;
joe_feubli 13:cc43e546f310 251 Power_ein = true;
joe_feubli 7:44f2f9fd2eeb 252 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 253 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 254 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 255 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 256 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 257 MyEpos5.Power(0);
joe_feubli 11:6e5ebe9aa84b 258 current_step = 2;//BEREIT;
joe_feubli 2:0f4e77a22fd9 259 }
joe_feubli 10:1c42bbd596dc 260
joe_feubli 8:98d4028cdea6 261
joe_feubli 6:3eaa420dc2f5 262 led1 = 0;
joe_feubli 2:0f4e77a22fd9 263 break;
joe_feubli 2:0f4e77a22fd9 264
joe_feubli 2:0f4e77a22fd9 265
joe_feubli 2:0f4e77a22fd9 266 case 2://BEREIT:
joe_feubli 6:3eaa420dc2f5 267 led2 = 1;
joe_feubli 8:98d4028cdea6 268 MyEpos5.SetDigOut(4,0);
joe_feubli 6:3eaa420dc2f5 269 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 8:98d4028cdea6 270 MyEpos5.SetDigOut(4,1);
joe_feubli 7:44f2f9fd2eeb 271 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 272 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 273 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 274 MyEpos3.Power(1);
joe_feubli 11:6e5ebe9aa84b 275 MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 276 MyEpos5.Power(1);
joe_feubli 14:bee23de5dff0 277 MyEpos6.Power(1);
joe_feubli 14:bee23de5dff0 278 MyEpos7.Power(1);
joe_feubli 12:9e90d7a5a8c6 279 einfahren = false;
joe_feubli 13:cc43e546f310 280 ausfahren_step = 1;
joe_feubli 13:cc43e546f310 281 current_step = 3;
joe_feubli 11:6e5ebe9aa84b 282 wait(0.5);
joe_feubli 6:3eaa420dc2f5 283 }
joe_feubli 14:bee23de5dff0 284 wait(0.1);
joe_feubli 14:bee23de5dff0 285 /********TEST********/ /*
joe_feubli 14:bee23de5dff0 286 MyEpos5.SetDigOut(4,1);
joe_feubli 11:6e5ebe9aa84b 287 wait_ms(10);
joe_feubli 14:bee23de5dff0 288 MyEpos1.Power(1);
joe_feubli 14:bee23de5dff0 289 MyEpos2.Power(1);
joe_feubli 14:bee23de5dff0 290 MyEpos3.Power(1);
joe_feubli 14:bee23de5dff0 291 MyEpos4.Power(1);
joe_feubli 14:bee23de5dff0 292 MyEpos5.Power(1);
joe_feubli 14:bee23de5dff0 293 MyEpos6.Power(1);
joe_feubli 14:bee23de5dff0 294 MyEpos7.Power(1);
joe_feubli 14:bee23de5dff0 295 einfahren = false;
joe_feubli 14:bee23de5dff0 296 ausfahren_step = 1;
joe_feubli 14:bee23de5dff0 297 current_step = 3;
joe_feubli 14:bee23de5dff0 298 */ /********TEST*******/
joe_feubli 9:039318b9096e 299 led2 = 0;
joe_feubli 2:0f4e77a22fd9 300 break;
joe_feubli 13:cc43e546f310 301
joe_feubli 13:cc43e546f310 302 case 3://ABLAUF
joe_feubli 12:9e90d7a5a8c6 303 // pc.printf("Läuft...\n");
joe_feubli 7:44f2f9fd2eeb 304 led3 = 1;
joe_feubli 6:3eaa420dc2f5 305 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 306 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 307 case 1:
joe_feubli 13:cc43e546f310 308 MyEpos5.MoveAbsolute(-150000,800*speed,100*speed,100*speed); //Kopf
joe_feubli 6:3eaa420dc2f5 309 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 310 wait(1);
joe_feubli 6:3eaa420dc2f5 311 break;
joe_feubli 6:3eaa420dc2f5 312
joe_feubli 6:3eaa420dc2f5 313 case 2:
joe_feubli 6:3eaa420dc2f5 314 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 315 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 316 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 317 MyEpos2.MoveAbsolute(310000,500*speed,100*speed,100*speed); //Panel Innen Links
joe_feubli 13:cc43e546f310 318 MyEpos4.MoveAbsolute(-310000,500*speed,100*speed,100*speed); //Panel Innen Rechts
joe_feubli 6:3eaa420dc2f5 319 ausfahren_step = 3;
joe_feubli 10:1c42bbd596dc 320 wait(1);
joe_feubli 6:3eaa420dc2f5 321 }
joe_feubli 14:bee23de5dff0 322 wait(0.1);
joe_feubli 6:3eaa420dc2f5 323 break;
joe_feubli 6:3eaa420dc2f5 324
joe_feubli 6:3eaa420dc2f5 325 case 3:
joe_feubli 13:cc43e546f310 326 if (MyEpos2.GetActualPos() > 280000 and MyEpos4.GetActualPos() < -280000){
joe_feubli 13:cc43e546f310 327 MyEpos1.MoveAbsolute(318000,500*speed,100*speed,100*speed); //Panel Aussen Links
joe_feubli 13:cc43e546f310 328 MyEpos3.MoveAbsolute(-318000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 329
joe_feubli 6:3eaa420dc2f5 330 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 331 wait(1);
joe_feubli 6:3eaa420dc2f5 332 }
joe_feubli 14:bee23de5dff0 333 wait(0.1);
joe_feubli 6:3eaa420dc2f5 334 break;
joe_feubli 6:3eaa420dc2f5 335
joe_feubli 6:3eaa420dc2f5 336 case 4:
joe_feubli 6:3eaa420dc2f5 337 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 14:bee23de5dff0 338 MyEpos6.MoveAbsolute(-76000,500*speed,100*speed,100*speed); //Bohrgestell
joe_feubli 14:bee23de5dff0 339 ausfahren_step = 5;
joe_feubli 14:bee23de5dff0 340 wait(1);
joe_feubli 14:bee23de5dff0 341 }
joe_feubli 14:bee23de5dff0 342 wait(0.1);
joe_feubli 14:bee23de5dff0 343 break;
joe_feubli 14:bee23de5dff0 344
joe_feubli 14:bee23de5dff0 345
joe_feubli 14:bee23de5dff0 346 case 5:
joe_feubli 14:bee23de5dff0 347 if (MyEpos6.TargetReached()==1){
joe_feubli 14:bee23de5dff0 348 MyEpos7.MoveAbsolute(700000,2000*speed,500*speed,500*speed); //Bohrer
joe_feubli 14:bee23de5dff0 349 ausfahren_step = 6;
joe_feubli 14:bee23de5dff0 350 wait(1);
joe_feubli 14:bee23de5dff0 351 }
joe_feubli 14:bee23de5dff0 352 wait(0.1);
joe_feubli 14:bee23de5dff0 353 break;
joe_feubli 14:bee23de5dff0 354
joe_feubli 14:bee23de5dff0 355
joe_feubli 14:bee23de5dff0 356 case 6:
joe_feubli 14:bee23de5dff0 357 if (MyEpos7.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 358 einfahren = true;
joe_feubli 6:3eaa420dc2f5 359 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 360 wait(1);
joe_feubli 6:3eaa420dc2f5 361 }
joe_feubli 14:bee23de5dff0 362 wait(0.1);
joe_feubli 14:bee23de5dff0 363 // while(buffering == 0){//für Fotograf
joe_feubli 14:bee23de5dff0 364 // }
joe_feubli 6:3eaa420dc2f5 365 break;
joe_feubli 14:bee23de5dff0 366
joe_feubli 14:bee23de5dff0 367 /*
joe_feubli 14:bee23de5dff0 368 case 4:
joe_feubli 14:bee23de5dff0 369 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 14:bee23de5dff0 370 einfahren = true;
joe_feubli 14:bee23de5dff0 371 einfahren_step = 3;
joe_feubli 14:bee23de5dff0 372 wait(1);
joe_feubli 14:bee23de5dff0 373 }
joe_feubli 14:bee23de5dff0 374 wait(0.1);
joe_feubli 14:bee23de5dff0 375 break;
joe_feubli 14:bee23de5dff0 376 */
joe_feubli 13:cc43e546f310 377
joe_feubli 13:cc43e546f310 378
joe_feubli 12:9e90d7a5a8c6 379
joe_feubli 12:9e90d7a5a8c6 380 }//switch (ausfahren_step)
joe_feubli 12:9e90d7a5a8c6 381 }//if (einfahren == false)
joe_feubli 6:3eaa420dc2f5 382
joe_feubli 6:3eaa420dc2f5 383 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 384 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 385 switch (einfahren_step){
joe_feubli 14:bee23de5dff0 386
joe_feubli 6:3eaa420dc2f5 387 case 1:
joe_feubli 14:bee23de5dff0 388 MyEpos7.MoveAbsolute(0,2000*speed,500*speed,500*speed); //Bohrer
joe_feubli 6:3eaa420dc2f5 389 einfahren_step = 2;
joe_feubli 12:9e90d7a5a8c6 390 wait(0.1);
joe_feubli 6:3eaa420dc2f5 391 break;
joe_feubli 14:bee23de5dff0 392
joe_feubli 14:bee23de5dff0 393 case 2:
joe_feubli 14:bee23de5dff0 394 if (MyEpos7.TargetReached()==1){
joe_feubli 14:bee23de5dff0 395 MyEpos6.MoveAbsolute(0,500*speed,100*speed,100*speed); //Bohrgestell
joe_feubli 14:bee23de5dff0 396 einfahren_step = 3;
joe_feubli 14:bee23de5dff0 397 wait(1);
joe_feubli 14:bee23de5dff0 398 }
joe_feubli 14:bee23de5dff0 399 wait(0.1);
joe_feubli 14:bee23de5dff0 400 break;
joe_feubli 6:3eaa420dc2f5 401
joe_feubli 14:bee23de5dff0 402 case 3:
joe_feubli 16:e5343d6c9fb2 403 if (MyEpos6.TargetReached()==1){
joe_feubli 14:bee23de5dff0 404 MyEpos1.MoveAbsolute(10000,500*speed,100*speed,100*speed); //Panel Aussen Links
joe_feubli 14:bee23de5dff0 405 MyEpos3.MoveAbsolute(-10000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
joe_feubli 14:bee23de5dff0 406 einfahren_step = 4;
joe_feubli 16:e5343d6c9fb2 407 wait(1);
joe_feubli 16:e5343d6c9fb2 408 }
joe_feubli 14:bee23de5dff0 409 wait(0.1);
joe_feubli 14:bee23de5dff0 410 break;
joe_feubli 14:bee23de5dff0 411
joe_feubli 14:bee23de5dff0 412 case 4:
joe_feubli 13:cc43e546f310 413 if (MyEpos1.GetActualPos() < 20000 and MyEpos3.GetActualPos() > -20000){
joe_feubli 8:98d4028cdea6 414 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 415 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 13:cc43e546f310 416 MyEpos2.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Links
joe_feubli 13:cc43e546f310 417 MyEpos4.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Rechts
joe_feubli 14:bee23de5dff0 418 einfahren_step = 5;
joe_feubli 12:9e90d7a5a8c6 419 wait(0.1);
joe_feubli 6:3eaa420dc2f5 420 }
joe_feubli 14:bee23de5dff0 421 wait(0.1);
joe_feubli 6:3eaa420dc2f5 422 break;
joe_feubli 6:3eaa420dc2f5 423
joe_feubli 14:bee23de5dff0 424 case 5:
joe_feubli 11:6e5ebe9aa84b 425 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
joe_feubli 13:cc43e546f310 426 MyEpos5.MoveAbsolute(1000,500*speed,100*speed,100*speed); //Kopf
joe_feubli 6:3eaa420dc2f5 427 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 428 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 429 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 430 MyEpos4.Power(0);
joe_feubli 14:bee23de5dff0 431 einfahren_step = 6;
joe_feubli 12:9e90d7a5a8c6 432 wait(0.1);
joe_feubli 6:3eaa420dc2f5 433 }
joe_feubli 14:bee23de5dff0 434 wait(0.1);
joe_feubli 6:3eaa420dc2f5 435 break;
joe_feubli 6:3eaa420dc2f5 436
joe_feubli 14:bee23de5dff0 437 case 6:
joe_feubli 6:3eaa420dc2f5 438 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 439 einfahren = false;
joe_feubli 6:3eaa420dc2f5 440 current_step = 2;
joe_feubli 6:3eaa420dc2f5 441 wait(1);
joe_feubli 6:3eaa420dc2f5 442 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 443 }
joe_feubli 14:bee23de5dff0 444 wait(0.1);
joe_feubli 10:1c42bbd596dc 445 break;
joe_feubli 10:1c42bbd596dc 446
joe_feubli 12:9e90d7a5a8c6 447 }//switch(einfahren_step)
joe_feubli 12:9e90d7a5a8c6 448 }//if einfahren == true)
joe_feubli 7:44f2f9fd2eeb 449 led3 = 0;
joe_feubli 13:cc43e546f310 450 break;
joe_feubli 13:cc43e546f310 451
joe_feubli 13:cc43e546f310 452 case 5://USV:
joe_feubli 13:cc43e546f310 453 pc.printf("USV\n");
joe_feubli 13:cc43e546f310 454 led4 = 1;
joe_feubli 13:cc43e546f310 455 MyEpos5.SetDigOut(4,1);
joe_feubli 13:cc43e546f310 456 switch (usv_step){
joe_feubli 13:cc43e546f310 457 case 0:
joe_feubli 16:e5343d6c9fb2 458 pc.printf("USV 0\n");
joe_feubli 14:bee23de5dff0 459 wait(0.1);
joe_feubli 13:cc43e546f310 460 MyEpos1.Power(1);
joe_feubli 13:cc43e546f310 461 MyEpos2.Power(1);
joe_feubli 13:cc43e546f310 462 MyEpos3.Power(1);
joe_feubli 13:cc43e546f310 463 MyEpos4.Power(1);
joe_feubli 13:cc43e546f310 464 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 465 MyEpos6.Power(1);
joe_feubli 13:cc43e546f310 466 MyEpos7.Power(1);
joe_feubli 13:cc43e546f310 467 usv_step = 1;
joe_feubli 14:bee23de5dff0 468 wait(0.1);
joe_feubli 13:cc43e546f310 469 break;
joe_feubli 13:cc43e546f310 470
joe_feubli 13:cc43e546f310 471 case 1:
joe_feubli 16:e5343d6c9fb2 472 pc.printf("USV 1\n");
joe_feubli 13:cc43e546f310 473 MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
joe_feubli 13:cc43e546f310 474 MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
joe_feubli 14:bee23de5dff0 475 MyEpos7.MoveAbsolute(0,2000,500,500); //Bohrer
joe_feubli 13:cc43e546f310 476 wait(0.1);
joe_feubli 13:cc43e546f310 477
joe_feubli 14:bee23de5dff0 478 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1 and MyEpos7.TargetReached()==1){
joe_feubli 13:cc43e546f310 479 usv_step = 2;
joe_feubli 13:cc43e546f310 480 wait(0.1);
joe_feubli 13:cc43e546f310 481 }
joe_feubli 13:cc43e546f310 482 break;
joe_feubli 13:cc43e546f310 483
joe_feubli 13:cc43e546f310 484 case 2:
joe_feubli 14:bee23de5dff0 485 pc.printf("USV 2\n");
joe_feubli 13:cc43e546f310 486 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 487 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 13:cc43e546f310 488
joe_feubli 13:cc43e546f310 489 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 13:cc43e546f310 490 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 14:bee23de5dff0 491
joe_feubli 14:bee23de5dff0 492 MyEpos6.MoveAbsolute(0,500,100,100); //Bohrgestell
joe_feubli 13:cc43e546f310 493 wait(0.1);
joe_feubli 13:cc43e546f310 494
joe_feubli 16:e5343d6c9fb2 495 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 and MyEpos6.TargetReached()==1){
joe_feubli 13:cc43e546f310 496 usv_step = 3;
joe_feubli 13:cc43e546f310 497 wait(0.1);
joe_feubli 13:cc43e546f310 498 }
joe_feubli 13:cc43e546f310 499 break;
joe_feubli 13:cc43e546f310 500
joe_feubli 13:cc43e546f310 501 case 3:
joe_feubli 14:bee23de5dff0 502 pc.printf("USV 3\n");
joe_feubli 14:bee23de5dff0 503 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 14:bee23de5dff0 504 wait(0.1);
joe_feubli 14:bee23de5dff0 505 if (MyEpos5.TargetReached()==1){
joe_feubli 14:bee23de5dff0 506 usv_step = 4;
joe_feubli 13:cc43e546f310 507 wait(0.1);
joe_feubli 14:bee23de5dff0 508 }
joe_feubli 13:cc43e546f310 509 break;
joe_feubli 13:cc43e546f310 510
joe_feubli 13:cc43e546f310 511 case 4:
joe_feubli 14:bee23de5dff0 512 pc.printf("USV 4\n");
joe_feubli 13:cc43e546f310 513 MyEpos1.Power(0);
joe_feubli 13:cc43e546f310 514 MyEpos2.Power(0);
joe_feubli 13:cc43e546f310 515 MyEpos3.Power(0);
joe_feubli 13:cc43e546f310 516 MyEpos4.Power(0);
joe_feubli 13:cc43e546f310 517 MyEpos5.Power(0);
joe_feubli 13:cc43e546f310 518 MyEpos6.Power(0);
joe_feubli 13:cc43e546f310 519 MyEpos7.Power(0);
joe_feubli 13:cc43e546f310 520 usv_step = 5;
joe_feubli 13:cc43e546f310 521 wait(0.1);
joe_feubli 13:cc43e546f310 522 break;
joe_feubli 13:cc43e546f310 523
joe_feubli 13:cc43e546f310 524 case 5:
joe_feubli 14:bee23de5dff0 525 pc.printf("USV 5\n");
joe_feubli 13:cc43e546f310 526 if(buffering == 1){
joe_feubli 13:cc43e546f310 527 led3 = 1;
joe_feubli 13:cc43e546f310 528 } else {
joe_feubli 13:cc43e546f310 529 led3 = 0;
joe_feubli 13:cc43e546f310 530 Power_ein = true;
joe_feubli 13:cc43e546f310 531 current_step=1;
joe_feubli 13:cc43e546f310 532 usv_step=0;
joe_feubli 13:cc43e546f310 533 ausfahren_step=1;
joe_feubli 13:cc43e546f310 534 einfahren_step=1;
joe_feubli 14:bee23de5dff0 535 wait(0.1);
joe_feubli 14:bee23de5dff0 536 break;
joe_feubli 13:cc43e546f310 537 }
joe_feubli 13:cc43e546f310 538 wait(1);
joe_feubli 13:cc43e546f310 539 break;
joe_feubli 13:cc43e546f310 540 }//switch(usv_step)
joe_feubli 13:cc43e546f310 541 led4 = 0;
joe_feubli 13:cc43e546f310 542 break;
joe_feubli 13:cc43e546f310 543
joe_feubli 14:bee23de5dff0 544 case 6: //Fehler Bohrer/Bohrgestell
joe_feubli 13:cc43e546f310 545 pc.printf("Fehler RESET Bohrer\n");
joe_feubli 14:bee23de5dff0 546 MyEpos7.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 547 if (MyEpos7.TargetReached() == 1){
joe_feubli 14:bee23de5dff0 548 MyEpos6.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 549 if(MyEpos6.TargetReached() == 1){
joe_feubli 14:bee23de5dff0 550 MyEpos5.Reset();
joe_feubli 14:bee23de5dff0 551 MyEpos5.Power(1);
joe_feubli 14:bee23de5dff0 552 MyEpos6.Reset();
joe_feubli 14:bee23de5dff0 553 MyEpos6.Power(1);
joe_feubli 14:bee23de5dff0 554 current_step = 1;
joe_feubli 14:bee23de5dff0 555 }
joe_feubli 14:bee23de5dff0 556 }
joe_feubli 13:cc43e546f310 557 //node1_homed = false;
joe_feubli 13:cc43e546f310 558 //node2_homed = false;
joe_feubli 14:bee23de5dff0 559 //current_step = 1;
ODEM 1:c53aafa72b36 560 break;
joe_feubli 12:9e90d7a5a8c6 561
joe_feubli 12:9e90d7a5a8c6 562 case 7:
joe_feubli 13:cc43e546f310 563 pc.printf("Fehler RESET\n");
joe_feubli 12:9e90d7a5a8c6 564 MyEpos1.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 565 MyEpos2.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 566 MyEpos3.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 567 MyEpos4.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 568 MyEpos5.MoveVelocity(0, 500, 500);
joe_feubli 14:bee23de5dff0 569 wait(1);
joe_feubli 12:9e90d7a5a8c6 570 MyEpos1.Reset();
joe_feubli 12:9e90d7a5a8c6 571 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 572 MyEpos2.Reset();
joe_feubli 12:9e90d7a5a8c6 573 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 574 MyEpos3.Reset();
joe_feubli 12:9e90d7a5a8c6 575 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 576 MyEpos4.Reset();
joe_feubli 12:9e90d7a5a8c6 577 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 578 MyEpos5.Reset();
joe_feubli 12:9e90d7a5a8c6 579 MyEpos5.Power(1);
joe_feubli 13:cc43e546f310 580 epos1_homing = false;
joe_feubli 13:cc43e546f310 581 epos3_homing = false;
joe_feubli 12:9e90d7a5a8c6 582 node1_homed = false;
joe_feubli 12:9e90d7a5a8c6 583 node2_homed = false;
joe_feubli 12:9e90d7a5a8c6 584 node3_homed = false;
joe_feubli 12:9e90d7a5a8c6 585 node4_homed = false;
joe_feubli 12:9e90d7a5a8c6 586 node5_homed = false;
joe_feubli 13:cc43e546f310 587 Power_ein = true;
joe_feubli 13:cc43e546f310 588 if(MyEpos2.GetActualPos() < 100000 or MyEpos4.GetActualPos() > -100000){
joe_feubli 14:bee23de5dff0 589 MyEpos1.Homing();
joe_feubli 14:bee23de5dff0 590 MyEpos3.Homing();
joe_feubli 14:bee23de5dff0 591 MyEpos1.MoveAbsolute(0,500,100,100);
joe_feubli 14:bee23de5dff0 592 MyEpos3.MoveAbsolute(0,500,100,100);
joe_feubli 13:cc43e546f310 593 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
joe_feubli 13:cc43e546f310 594 MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
joe_feubli 13:cc43e546f310 595 while (MyEpos2.TargetReached() != 1 or MyEpos4.TargetReached() != 1){
joe_feubli 13:cc43e546f310 596 if(MyEpos2.GetError() == 1 or MyEpos4.GetError() == 1){
joe_feubli 14:bee23de5dff0 597 //wait(0.1);
joe_feubli 13:cc43e546f310 598 break;
joe_feubli 13:cc43e546f310 599 }
joe_feubli 14:bee23de5dff0 600 //wait(0.1);
joe_feubli 13:cc43e546f310 601 }
joe_feubli 13:cc43e546f310 602 }
joe_feubli 14:bee23de5dff0 603 current_step = 6;
joe_feubli 12:9e90d7a5a8c6 604 wait(0.1);
joe_feubli 13:cc43e546f310 605
joe_feubli 12:9e90d7a5a8c6 606 break;
joe_feubli 12:9e90d7a5a8c6 607 }//switch (current_step)
joe_feubli 2:0f4e77a22fd9 608 }//while
joe_feubli 10:1c42bbd596dc 609 }//main