mmotor / Mbed 2 deprecated MarsRover_ExoMars

Programm mit Ablauf

Dependencies:   EPOS2 mbed-rtos mbed

Fork of Projekt_Kugelbahn by mmotor

main.cpp@10:1c42bbd596dc, 2016-10-17 (annotated)

Committer:
joe_feubli
Date:
Mon Oct 17 05:39:38 2016 +0000
Revision:
10:1c42bbd596dc
Parent:
9:039318b9096e
Child:
11:6e5ebe9aa84b
"funktionierendes" Programm mit USV und Buzzer -> ohne Motor4 & Fehlerauswertung

Who changed what in which revision?

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