Programm mit Ablauf
Dependencies: EPOS2 mbed-rtos mbed
Fork of Projekt_Kugelbahn by
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?
User | Revision | Line number | New 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 |