mmotor / Mbed 2 deprecated MarsRover_ExoMars

Programm mit Ablauf

Dependencies:   EPOS2 mbed-rtos mbed

Fork of Projekt_Kugelbahn by mmotor

main.cpp@8:98d4028cdea6, 2016-10-14 (annotated)

Committer:
joe_feubli
Date:
Fri Oct 14 11:17:52 2016 +0000
Revision:
8:98d4028cdea6
Parent:
7:44f2f9fd2eeb
Child:
9:039318b9096e
Funktion bis und mit USV (ohne Achse 4)

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 2:0f4e77a22fd9 12 /*
ODEM 0:aaf583a75b97 13 int velocity = 3000;
ODEM 0:aaf583a75b97 14 int acceleration = 50000;
ODEM 0:aaf583a75b97 15 int deceleration = 50000;
ODEM 0:aaf583a75b97 16
ODEM 0:aaf583a75b97 17 int anzahl_zyklen = 1000000;
ODEM 0:aaf583a75b97 18 int absolvierte_zyklen = 0;
ODEM 0:aaf583a75b97 19 int ActualPos = 0;
ODEM 0:aaf583a75b97 20 int DemandPos = 0;
ODEM 0:aaf583a75b97 21 int ActualCurrent = 0;
ODEM 0:aaf583a75b97 22 int DemandCurrent = 0;
ODEM 0:aaf583a75b97 23 int counter=0;
ODEM 1:c53aafa72b36 24 int counter2=0;
ODEM 0:aaf583a75b97 25 bool start = true;
ODEM 4:8fb30d6c2855 26
ODEM 1:c53aafa72b36 27 int counter_s = false;
ODEM 1:c53aafa72b36 28 int counter_s2 = false;
joe_feubli 3:fbc9de097e4f 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 8:98d4028cdea6 66 buffering.rise(&interrupt_usv); //Interrupt für USV
joe_feubli 7:44f2f9fd2eeb 67
joe_feubli 7:44f2f9fd2eeb 68 wait(0.5); //Wartezeit bis Epos2 70/10 aufgestartet ist
joe_feubli 7:44f2f9fd2eeb 69 led2 = 1;
joe_feubli 7:44f2f9fd2eeb 70 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 71 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 72 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 73 led4 = 1;
joe_feubli 7:44f2f9fd2eeb 74 wait(0.5);
ODEM 0:aaf583a75b97 75
ODEM 0:aaf583a75b97 76 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 77
ODEM 0:aaf583a75b97 78 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 79
ODEM 0:aaf583a75b97 80 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 81 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 82
joe_feubli 2:0f4e77a22fd9 83 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 84 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 85 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 86 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 87 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 88 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 89 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 90
ODEM 0:aaf583a75b97 91 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 2:0f4e77a22fd9 92
joe_feubli 6:3eaa420dc2f5 93 MyEpos1.Reset();
joe_feubli 6:3eaa420dc2f5 94 MyEpos2.Reset();
joe_feubli 6:3eaa420dc2f5 95 MyEpos3.Reset();
joe_feubli 6:3eaa420dc2f5 96 MyEpos4.Reset();
joe_feubli 6:3eaa420dc2f5 97 MyEpos5.Reset();
joe_feubli 6:3eaa420dc2f5 98 MyEpos6.Reset();
joe_feubli 6:3eaa420dc2f5 99 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 100 wait(0.1);
joe_feubli 6:3eaa420dc2f5 101
joe_feubli 7:44f2f9fd2eeb 102 led1 = 0;
joe_feubli 7:44f2f9fd2eeb 103 led2 = 0;
joe_feubli 7:44f2f9fd2eeb 104 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 105 led4 = 0;
joe_feubli 8:98d4028cdea6 106 MyEpos5.SetDigOut(4, 1);
joe_feubli 7:44f2f9fd2eeb 107
joe_feubli 6:3eaa420dc2f5 108 /* mySwitch1.mode(PullUp);
joe_feubli 6:3eaa420dc2f5 109 mySwitch2.mode(PullUp);
joe_feubli 6:3eaa420dc2f5 110 mySwitch3.mode(PullUp);
joe_feubli 6:3eaa420dc2f5 111 */
joe_feubli 2:0f4e77a22fd9 112 //************** Initialisierung **************//
joe_feubli 2:0f4e77a22fd9 113
joe_feubli 5:524e83ec5174 114 /* HOMING METHODEN:
joe_feubli 5:524e83ec5174 115 Homing Method 7 :Home Switch Positive Speed & Index
joe_feubli 5:524e83ec5174 116 Homing Method 11:Home Switch Negative Speed & Index
joe_feubli 5:524e83ec5174 117 Homing Method 23: Home Switch Positive Speed
joe_feubli 5:524e83ec5174 118 Homing Method 27: Home Switch Negative Speed
joe_feubli 6:3eaa420dc2f5 119 Homing Method -3: Current Threshold Positive Speed
joe_feubli 6:3eaa420dc2f5 120 Homing Method -4: Current Threshold Negative Speed
joe_feubli 6:3eaa420dc2f5 121
joe_feubli 5:524e83ec5174 122 */
joe_feubli 5:524e83ec5174 123 //Motortype 10: EC-Motor
joe_feubli 5:524e83ec5174 124
joe_feubli 6:3eaa420dc2f5 125 /* 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 6:3eaa420dc2f5 126 MyEpos1.SetHomingPar(-4, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 2:0f4e77a22fd9 127 MyEpos1.Reset();
joe_feubli 2:0f4e77a22fd9 128
joe_feubli 6:3eaa420dc2f5 129 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 6:3eaa420dc2f5 130 MyEpos2.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 131 MyEpos2.Reset();
joe_feubli 2:0f4e77a22fd9 132
joe_feubli 6:3eaa420dc2f5 133 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 6:3eaa420dc2f5 134 MyEpos3.SetHomingPar(-3, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 2:0f4e77a22fd9 135 MyEpos3.Reset();
joe_feubli 2:0f4e77a22fd9 136
joe_feubli 6:3eaa420dc2f5 137 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 5:524e83ec5174 138 MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 139 MyEpos4.Reset();
joe_feubli 2:0f4e77a22fd9 140
joe_feubli 6:3eaa420dc2f5 141 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 6:3eaa420dc2f5 142 MyEpos5.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 143 MyEpos5.Reset();
joe_feubli 2:0f4e77a22fd9 144
joe_feubli 5:524e83ec5174 145 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 5:524e83ec5174 146 MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT
joe_feubli 2:0f4e77a22fd9 147 MyEpos6.Reset();
joe_feubli 2:0f4e77a22fd9 148
joe_feubli 2:0f4e77a22fd9 149 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 5:524e83ec5174 150 MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 2:0f4e77a22fd9 151 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 152 */
ODEM 0:aaf583a75b97 153
ODEM 0:aaf583a75b97 154 while(1){
joe_feubli 2:0f4e77a22fd9 155 switch (current_step){
joe_feubli 2:0f4e77a22fd9 156 case 1://HOMING:
joe_feubli 6:3eaa420dc2f5 157 led1 = 1;
joe_feubli 5:524e83ec5174 158
joe_feubli 6:3eaa420dc2f5 159 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 160 MyEpos2.Power(1);
joe_feubli 5:524e83ec5174 161 MyEpos3.Power(1);
joe_feubli 5:524e83ec5174 162 MyEpos4.Power(1);
joe_feubli 5:524e83ec5174 163 MyEpos5.Power(1);
joe_feubli 5:524e83ec5174 164 MyEpos6.Power(1);
joe_feubli 6:3eaa420dc2f5 165 MyEpos7.Power(1);
joe_feubli 6:3eaa420dc2f5 166
joe_feubli 6:3eaa420dc2f5 167 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 168
joe_feubli 7:44f2f9fd2eeb 169 if (node5_homed == false){
joe_feubli 7:44f2f9fd2eeb 170 MyEpos5.Homing(); //Kopf
joe_feubli 7:44f2f9fd2eeb 171 node5_homed = true;
joe_feubli 7:44f2f9fd2eeb 172 break;
joe_feubli 7:44f2f9fd2eeb 173 }
joe_feubli 7:44f2f9fd2eeb 174
joe_feubli 6:3eaa420dc2f5 175 if (node1_homed == false){
joe_feubli 6:3eaa420dc2f5 176 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 6:3eaa420dc2f5 177 node1_homed = true;
joe_feubli 6:3eaa420dc2f5 178 break;
joe_feubli 6:3eaa420dc2f5 179 }
joe_feubli 6:3eaa420dc2f5 180
joe_feubli 6:3eaa420dc2f5 181 if (node3_homed == false){
joe_feubli 6:3eaa420dc2f5 182 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 6:3eaa420dc2f5 183 node3_homed = true;
joe_feubli 6:3eaa420dc2f5 184 break;
joe_feubli 6:3eaa420dc2f5 185 }
joe_feubli 6:3eaa420dc2f5 186
joe_feubli 6:3eaa420dc2f5 187 if (node7_homed == false){
joe_feubli 6:3eaa420dc2f5 188 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 189 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 190 break;
joe_feubli 6:3eaa420dc2f5 191 }
joe_feubli 6:3eaa420dc2f5 192
joe_feubli 7:44f2f9fd2eeb 193 if (node1_homed == true and node2_homed == false and node5_homed == true){
joe_feubli 7:44f2f9fd2eeb 194 MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 195 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 7:44f2f9fd2eeb 196 MyEpos5.MoveAbsolute(500,100,50,50); //Kopf
joe_feubli 7:44f2f9fd2eeb 197 wait(3);
joe_feubli 6:3eaa420dc2f5 198 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 199 break;
joe_feubli 2:0f4e77a22fd9 200 }
joe_feubli 6:3eaa420dc2f5 201
joe_feubli 6:3eaa420dc2f5 202 if (node3_homed == true and node4_homed == false){
joe_feubli 6:3eaa420dc2f5 203 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 204 node4_homed = true;
joe_feubli 6:3eaa420dc2f5 205 break;
joe_feubli 6:3eaa420dc2f5 206 }
joe_feubli 6:3eaa420dc2f5 207
joe_feubli 6:3eaa420dc2f5 208 if (node7_homed == true and node6_homed == false){
joe_feubli 6:3eaa420dc2f5 209 MyEpos6.Homing();//Bohrgestell
joe_feubli 6:3eaa420dc2f5 210 node6_homed = true;
joe_feubli 6:3eaa420dc2f5 211 break;
joe_feubli 6:3eaa420dc2f5 212 }
joe_feubli 6:3eaa420dc2f5 213 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 214 current_step = 2;//BEREIT;
joe_feubli 7:44f2f9fd2eeb 215 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 216 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 217 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 218 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 219 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 220 MyEpos5.Power(0);
joe_feubli 2:0f4e77a22fd9 221 }
joe_feubli 8:98d4028cdea6 222
joe_feubli 6:3eaa420dc2f5 223 led1 = 0;
joe_feubli 2:0f4e77a22fd9 224 break;
joe_feubli 2:0f4e77a22fd9 225
joe_feubli 2:0f4e77a22fd9 226
joe_feubli 2:0f4e77a22fd9 227 case 2://BEREIT:
joe_feubli 6:3eaa420dc2f5 228 led2 = 1;
joe_feubli 8:98d4028cdea6 229 MyEpos5.SetDigOut(4,0);
joe_feubli 6:3eaa420dc2f5 230 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 8:98d4028cdea6 231 MyEpos5.SetDigOut(4,1);
joe_feubli 6:3eaa420dc2f5 232 current_step = 6;
joe_feubli 7:44f2f9fd2eeb 233 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 234 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 235 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 236 MyEpos3.Power(1);
joe_feubli 6:3eaa420dc2f5 237 MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 238 MyEpos5.Power(1);
joe_feubli 6:3eaa420dc2f5 239 }
joe_feubli 6:3eaa420dc2f5 240 wait_ms(10);
joe_feubli 5:524e83ec5174 241 /* while (MyEpos6.GetDigIn(1)!=1) { //warten solange Buzzer nicht gedrückt
joe_feubli 2:0f4e77a22fd9 242 wait_ms(10);
joe_feubli 2:0f4e77a22fd9 243 }
joe_feubli 3:fbc9de097e4f 244 current_step = 3;//START_AUSFAHREN
joe_feubli 6:3eaa420dc2f5 245 current_step = 6; //Test
joe_feubli 6:3eaa420dc2f5 246 */ led2 = 0;
joe_feubli 2:0f4e77a22fd9 247 break;
ODEM 1:c53aafa72b36 248
joe_feubli 2:0f4e77a22fd9 249
joe_feubli 7:44f2f9fd2eeb 250 /* case 3://START_AUSFAHREN:
joe_feubli 6:3eaa420dc2f5 251 led3 = 1;
joe_feubli 6:3eaa420dc2f5 252 if(ausfahren == true){
joe_feubli 3:fbc9de097e4f 253 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 254 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 255 MyEpos2.MoveAbsolute(500,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 256 MyEpos4.MoveAbsolute(500,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 257 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 258 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 259 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 260 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 261 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 262 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 263 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 264 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 265 ausfahren = false;
joe_feubli 3:fbc9de097e4f 266 einfahren = true;
joe_feubli 3:fbc9de097e4f 267 current_step = 4;//START_EINFAHREN
joe_feubli 3:fbc9de097e4f 268 }
joe_feubli 3:fbc9de097e4f 269 }
joe_feubli 3:fbc9de097e4f 270 }
joe_feubli 3:fbc9de097e4f 271 }
joe_feubli 3:fbc9de097e4f 272 }
joe_feubli 3:fbc9de097e4f 273 }
joe_feubli 6:3eaa420dc2f5 274 led3 = 0;
joe_feubli 2:0f4e77a22fd9 275 break;
ODEM 1:c53aafa72b36 276
joe_feubli 3:fbc9de097e4f 277 case 4://Start_EINFAHREN
joe_feubli 6:3eaa420dc2f5 278 led4 = 1;
joe_feubli 6:3eaa420dc2f5 279 if(einfahren == true){
joe_feubli 3:fbc9de097e4f 280 MyEpos7.MoveAbsolute(0,200,500,500); //Bohrer
joe_feubli 3:fbc9de097e4f 281 if (MyEpos7.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 282 MyEpos6.MoveAbsolute(0,200,500,500); //Bohrgestell
joe_feubli 3:fbc9de097e4f 283 if (MyEpos6.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 284 MyEpos1.MoveAbsolute(0,200,500,500); //Panel_Aussen_Links
joe_feubli 3:fbc9de097e4f 285 MyEpos3.MoveAbsolute(0,200,500,500); //Panel_Aussen_Rechts
joe_feubli 3:fbc9de097e4f 286 if (MyEpos1.TargetReached()==1 && MyEpos3.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 287 MyEpos2.MoveAbsolute(0,200,500,500); //Panel_Innen_Links
joe_feubli 3:fbc9de097e4f 288 MyEpos4.MoveAbsolute(0,200,500,500); //Panel_Innen_Rechts
joe_feubli 3:fbc9de097e4f 289 if (MyEpos2.TargetReached()==1 && MyEpos4.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 290 MyEpos5.MoveAbsolute(0,200,500,500); //Kopf
joe_feubli 3:fbc9de097e4f 291 if (MyEpos5.TargetReached()==1){
joe_feubli 3:fbc9de097e4f 292 einfahren = false;
joe_feubli 3:fbc9de097e4f 293 current_step = 2;//Bereit
joe_feubli 3:fbc9de097e4f 294 }
joe_feubli 3:fbc9de097e4f 295 }
joe_feubli 3:fbc9de097e4f 296 }
joe_feubli 3:fbc9de097e4f 297 }
joe_feubli 3:fbc9de097e4f 298 }
joe_feubli 3:fbc9de097e4f 299 }
joe_feubli 6:3eaa420dc2f5 300 led4 = 0;
joe_feubli 3:fbc9de097e4f 301 break;
joe_feubli 3:fbc9de097e4f 302
joe_feubli 7:44f2f9fd2eeb 303 */
joe_feubli 3:fbc9de097e4f 304 case 5://USV:
joe_feubli 7:44f2f9fd2eeb 305 led4 = 1;
joe_feubli 8:98d4028cdea6 306 MyEpos5.SetDigOut(4,1);
joe_feubli 7:44f2f9fd2eeb 307 switch (usv_step){
joe_feubli 7:44f2f9fd2eeb 308 case 1:
joe_feubli 7:44f2f9fd2eeb 309 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 310 /* MyEpos1.Power(1);
joe_feubli 7:44f2f9fd2eeb 311 MyEpos2.Power(1);
joe_feubli 7:44f2f9fd2eeb 312 MyEpos3.Power(1);
joe_feubli 7:44f2f9fd2eeb 313 MyEpos4.Power(1);
joe_feubli 7:44f2f9fd2eeb 314 MyEpos5.Power(1);
joe_feubli 7:44f2f9fd2eeb 315 MyEpos6.Power(1);
joe_feubli 7:44f2f9fd2eeb 316 MyEpos7.Power(1);
joe_feubli 7:44f2f9fd2eeb 317 wait(1);
joe_feubli 7:44f2f9fd2eeb 318 */
joe_feubli 7:44f2f9fd2eeb 319 if (node1_homed == true){
joe_feubli 8:98d4028cdea6 320 MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
joe_feubli 7:44f2f9fd2eeb 321 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 322 }
joe_feubli 7:44f2f9fd2eeb 323 if (node3_homed == true){
joe_feubli 8:98d4028cdea6 324 MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
joe_feubli 7:44f2f9fd2eeb 325 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 326 }
joe_feubli 7:44f2f9fd2eeb 327 /* if (node7_homed == true){
joe_feubli 7:44f2f9fd2eeb 328 MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 7:44f2f9fd2eeb 329 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 330 }
joe_feubli 7:44f2f9fd2eeb 331 */ wait(0.1);
joe_feubli 8:98d4028cdea6 332 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 8:98d4028cdea6 333 usv_step = 2;
joe_feubli 8:98d4028cdea6 334 }
joe_feubli 7:44f2f9fd2eeb 335 break;
joe_feubli 7:44f2f9fd2eeb 336
joe_feubli 7:44f2f9fd2eeb 337 case 2:
joe_feubli 8:98d4028cdea6 338 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 339 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 7:44f2f9fd2eeb 340 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 8:98d4028cdea6 341 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 342 /* if (node4_homed == true and MyEpos3.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 343 wait(1);
joe_feubli 7:44f2f9fd2eeb 344 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 7:44f2f9fd2eeb 345 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 346 }
joe_feubli 7:44f2f9fd2eeb 347 if (node6_homed == true and MyEpos7.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 348 wait(1);
joe_feubli 7:44f2f9fd2eeb 349 MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 7:44f2f9fd2eeb 350 wait_ms(10);
ODEM 4:8fb30d6c2855 351 }
joe_feubli 7:44f2f9fd2eeb 352 */ wait(1);
joe_feubli 7:44f2f9fd2eeb 353 if (MyEpos2.TargetReached()==1 /*and MyEpos4.TargetReached()==1 and MyEpos6.TargetReached()==1*/){
joe_feubli 7:44f2f9fd2eeb 354 usv_step = 3;
joe_feubli 3:fbc9de097e4f 355 }
joe_feubli 7:44f2f9fd2eeb 356 break;
joe_feubli 7:44f2f9fd2eeb 357
joe_feubli 7:44f2f9fd2eeb 358 case 3:
joe_feubli 7:44f2f9fd2eeb 359 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 7:44f2f9fd2eeb 360 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 361 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 362 usv_step = 4;
joe_feubli 7:44f2f9fd2eeb 363 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 364 }
joe_feubli 7:44f2f9fd2eeb 365 break;
joe_feubli 7:44f2f9fd2eeb 366
joe_feubli 7:44f2f9fd2eeb 367 case 4:
joe_feubli 8:98d4028cdea6 368 MyEpos1.Power(0);
joe_feubli 8:98d4028cdea6 369 MyEpos2.Power(0);
joe_feubli 8:98d4028cdea6 370 MyEpos3.Power(0);
joe_feubli 8:98d4028cdea6 371 MyEpos4.Power(0);
joe_feubli 8:98d4028cdea6 372 MyEpos5.Power(0);
joe_feubli 8:98d4028cdea6 373 MyEpos6.Power(0);
joe_feubli 8:98d4028cdea6 374 MyEpos7.Power(0);
joe_feubli 8:98d4028cdea6 375 usv_step = 5;
joe_feubli 7:44f2f9fd2eeb 376 break;
joe_feubli 7:44f2f9fd2eeb 377
joe_feubli 7:44f2f9fd2eeb 378 case 5:
joe_feubli 7:44f2f9fd2eeb 379 if(buffering == 1){
joe_feubli 7:44f2f9fd2eeb 380 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 381 } else {
joe_feubli 7:44f2f9fd2eeb 382 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 383 current_step=1;
joe_feubli 7:44f2f9fd2eeb 384 usv_step=1;
joe_feubli 7:44f2f9fd2eeb 385 ausfahren_step=1;
joe_feubli 7:44f2f9fd2eeb 386 einfahren_step=1;
joe_feubli 7:44f2f9fd2eeb 387 }
joe_feubli 7:44f2f9fd2eeb 388 /*if (buffering == 0){
joe_feubli 7:44f2f9fd2eeb 389 current_step = 1;
joe_feubli 7:44f2f9fd2eeb 390 usv_step = 1;
joe_feubli 7:44f2f9fd2eeb 391 }*/
joe_feubli 7:44f2f9fd2eeb 392 wait(1);
joe_feubli 7:44f2f9fd2eeb 393 break;
joe_feubli 3:fbc9de097e4f 394 }
joe_feubli 7:44f2f9fd2eeb 395 led4 = 0;
joe_feubli 2:0f4e77a22fd9 396 break;
ODEM 1:c53aafa72b36 397
joe_feubli 5:524e83ec5174 398 case 6://Test
joe_feubli 6:3eaa420dc2f5 399 // MyEpos5.Power(1);
joe_feubli 7:44f2f9fd2eeb 400 led3 = 1;
joe_feubli 6:3eaa420dc2f5 401 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 402 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 403 case 1:
joe_feubli 6:3eaa420dc2f5 404 MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 405 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 406 wait(1);
joe_feubli 6:3eaa420dc2f5 407 break;
joe_feubli 6:3eaa420dc2f5 408
joe_feubli 6:3eaa420dc2f5 409 case 2:
joe_feubli 6:3eaa420dc2f5 410 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 411 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 412 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 413 MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 414 ausfahren_step = 3;
joe_feubli 6:3eaa420dc2f5 415 wait(1);
joe_feubli 6:3eaa420dc2f5 416 }
joe_feubli 6:3eaa420dc2f5 417 break;
joe_feubli 6:3eaa420dc2f5 418
joe_feubli 6:3eaa420dc2f5 419 case 3:
joe_feubli 6:3eaa420dc2f5 420 if (MyEpos2.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 421 MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 422 MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 423
joe_feubli 6:3eaa420dc2f5 424 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 425 wait(1);
joe_feubli 6:3eaa420dc2f5 426 }
joe_feubli 6:3eaa420dc2f5 427 break;
joe_feubli 6:3eaa420dc2f5 428
joe_feubli 6:3eaa420dc2f5 429 case 4:
joe_feubli 6:3eaa420dc2f5 430 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 431 einfahren = true;
joe_feubli 6:3eaa420dc2f5 432 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 433 wait(1);
joe_feubli 6:3eaa420dc2f5 434 }
joe_feubli 6:3eaa420dc2f5 435 break;
joe_feubli 6:3eaa420dc2f5 436
joe_feubli 6:3eaa420dc2f5 437 }
joe_feubli 5:524e83ec5174 438 }
joe_feubli 6:3eaa420dc2f5 439
joe_feubli 6:3eaa420dc2f5 440 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 441 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 442 switch (einfahren_step){
joe_feubli 6:3eaa420dc2f5 443 case 1:
joe_feubli 8:98d4028cdea6 444 MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 445 MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 446 einfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 447 wait(1);
joe_feubli 6:3eaa420dc2f5 448 break;
joe_feubli 6:3eaa420dc2f5 449
joe_feubli 6:3eaa420dc2f5 450 case 2:
joe_feubli 6:3eaa420dc2f5 451 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 452 wait(1);
joe_feubli 8:98d4028cdea6 453 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 454 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 455 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 456 einfahren_step = 3;
joe_feubli 6:3eaa420dc2f5 457 wait(1);
joe_feubli 6:3eaa420dc2f5 458 }
joe_feubli 6:3eaa420dc2f5 459 break;
joe_feubli 6:3eaa420dc2f5 460
joe_feubli 6:3eaa420dc2f5 461 case 3:
joe_feubli 6:3eaa420dc2f5 462 if (MyEpos2.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 463 MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 464 wait(1);
joe_feubli 6:3eaa420dc2f5 465 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 466 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 467 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 468 MyEpos4.Power(0);
joe_feubli 6:3eaa420dc2f5 469 einfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 470 }
joe_feubli 6:3eaa420dc2f5 471 break;
joe_feubli 6:3eaa420dc2f5 472
joe_feubli 6:3eaa420dc2f5 473 case 4:
joe_feubli 6:3eaa420dc2f5 474 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 475 einfahren = false;
joe_feubli 6:3eaa420dc2f5 476 current_step = 2;
joe_feubli 6:3eaa420dc2f5 477 wait(1);
joe_feubli 6:3eaa420dc2f5 478 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 479 }
joe_feubli 6:3eaa420dc2f5 480 break;
joe_feubli 6:3eaa420dc2f5 481
joe_feubli 6:3eaa420dc2f5 482 }
joe_feubli 5:524e83ec5174 483 }
joe_feubli 7:44f2f9fd2eeb 484 led3 = 0;
ODEM 1:c53aafa72b36 485 break;
joe_feubli 7:44f2f9fd2eeb 486 }//case
joe_feubli 2:0f4e77a22fd9 487 }//while
joe_feubli 2:0f4e77a22fd9 488 }//main