mmotor / Mbed 2 deprecated MarsRover_ExoMars____

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EPOS2 mbed-rtos mbed

Fork of MarsRover_ExoMars by mmotor

main.cpp@12:9e90d7a5a8c6, 2016-10-18 (annotated)

Committer:
joe_feubli
Date:
Tue Oct 18 14:16:09 2016 +0000
Revision:
12:9e90d7a5a8c6
Parent:
11:6e5ebe9aa84b
Child:
13:cc43e546f310
Funktion mit Allem (USV, Fehler-Reset, Buzzer, Homing)

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 11:6e5ebe9aa84b 42 bool node4_homed = false;
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 12:9e90d7a5a8c6 47 bool Power_ein = true;
joe_feubli 12:9e90d7a5a8c6 48 bool node5_grundposition = false;
joe_feubli 12:9e90d7a5a8c6 49
joe_feubli 6:3eaa420dc2f5 50 int homing = false;
joe_feubli 6:3eaa420dc2f5 51 int ready = false;
joe_feubli 3:fbc9de097e4f 52 int ausfahren = false;
joe_feubli 6:3eaa420dc2f5 53 int einfahren = false;
joe_feubli 6:3eaa420dc2f5 54 int usv = false;
joe_feubli 6:3eaa420dc2f5 55 int current_step = 1; //HOMING
joe_feubli 6:3eaa420dc2f5 56 int ausfahren_step = 1;
joe_feubli 7:44f2f9fd2eeb 57 int einfahren_step = 1;
joe_feubli 12:9e90d7a5a8c6 58 int usv_step = 1;
joe_feubli 12:9e90d7a5a8c6 59 int ErrorState;
joe_feubli 6:3eaa420dc2f5 60
joe_feubli 6:3eaa420dc2f5 61 void interrupt_usv(){
joe_feubli 6:3eaa420dc2f5 62 current_step = 5;
joe_feubli 7:44f2f9fd2eeb 63 wait_ms(10);
joe_feubli 7:44f2f9fd2eeb 64 }
ODEM 4:8fb30d6c2855 65
joe_feubli 2:0f4e77a22fd9 66 int main(){ ///////// Main
joe_feubli 7:44f2f9fd2eeb 67 led1 = 1;
ODEM 0:aaf583a75b97 68 pc.baud(9600);
ODEM 1:c53aafa72b36 69
joe_feubli 8:98d4028cdea6 70 buffering.rise(&interrupt_usv); //Interrupt für USV
joe_feubli 7:44f2f9fd2eeb 71
joe_feubli 12:9e90d7a5a8c6 72 wait(0.5); //Wartezeit bis Epos2 24/5 aufgestartet ist
joe_feubli 7:44f2f9fd2eeb 73 led2 = 1;
joe_feubli 7:44f2f9fd2eeb 74 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 75 led3 = 1;
joe_feubli 7:44f2f9fd2eeb 76 wait(0.5);
joe_feubli 7:44f2f9fd2eeb 77 led4 = 1;
joe_feubli 10:1c42bbd596dc 78 wait(0.5);
joe_feubli 10:1c42bbd596dc 79
joe_feubli 10:1c42bbd596dc 80 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 81
ODEM 0:aaf583a75b97 82 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 83
ODEM 0:aaf583a75b97 84 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 85 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 86
joe_feubli 2:0f4e77a22fd9 87 EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 88 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 89 EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 90 EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 91 EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 92 EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
joe_feubli 2:0f4e77a22fd9 93 EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 94
ODEM 0:aaf583a75b97 95 pc.printf("Initialisation abgeschlossen\n");
joe_feubli 12:9e90d7a5a8c6 96 wait(0.1);
joe_feubli 2:0f4e77a22fd9 97
joe_feubli 6:3eaa420dc2f5 98 MyEpos1.Reset();
joe_feubli 6:3eaa420dc2f5 99 MyEpos2.Reset();
joe_feubli 6:3eaa420dc2f5 100 MyEpos3.Reset();
joe_feubli 6:3eaa420dc2f5 101 MyEpos4.Reset();
joe_feubli 6:3eaa420dc2f5 102 MyEpos5.Reset();
joe_feubli 6:3eaa420dc2f5 103 MyEpos6.Reset();
joe_feubli 6:3eaa420dc2f5 104 MyEpos7.Reset();
joe_feubli 6:3eaa420dc2f5 105 wait(0.1);
joe_feubli 6:3eaa420dc2f5 106
joe_feubli 7:44f2f9fd2eeb 107 led1 = 0;
joe_feubli 7:44f2f9fd2eeb 108 led2 = 0;
joe_feubli 7:44f2f9fd2eeb 109 led3 = 0;
joe_feubli 7:44f2f9fd2eeb 110 led4 = 0;
joe_feubli 8:98d4028cdea6 111 MyEpos5.SetDigOut(4, 1);
joe_feubli 7:44f2f9fd2eeb 112
joe_feubli 10:1c42bbd596dc 113 /* usv_ready.mode(PullUp);
joe_feubli 10:1c42bbd596dc 114 buffering.mode(PullUp);
joe_feubli 10:1c42bbd596dc 115 replace_battery.mode(PullUp);
joe_feubli 10:1c42bbd596dc 116 */
joe_feubli 10:1c42bbd596dc 117 //************** Initialisierung **************//
joe_feubli 10:1c42bbd596dc 118
joe_feubli 10:1c42bbd596dc 119 /* HOMING METHODEN:
joe_feubli 10:1c42bbd596dc 120 Homing Method 7 :Home Switch Positive Speed & Index
joe_feubli 10:1c42bbd596dc 121 Homing Method 11:Home Switch Negative Speed & Index
joe_feubli 10:1c42bbd596dc 122 Homing Method 23: Home Switch Positive Speed
joe_feubli 10:1c42bbd596dc 123 Homing Method 27: Home Switch Negative Speed
joe_feubli 10:1c42bbd596dc 124 Homing Method -3: Current Threshold Positive Speed
joe_feubli 10:1c42bbd596dc 125 Homing Method -4: Current Threshold Negative Speed
joe_feubli 10:1c42bbd596dc 126
joe_feubli 10:1c42bbd596dc 127 */
joe_feubli 10:1c42bbd596dc 128 //Motortype 10: EC-Motor
joe_feubli 10:1c42bbd596dc 129
joe_feubli 10:1c42bbd596dc 130 /* 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 131 MyEpos1.SetHomingPar(-4, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 10:1c42bbd596dc 132 MyEpos1.Reset();
joe_feubli 10:1c42bbd596dc 133
joe_feubli 10:1c42bbd596dc 134 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 135 MyEpos2.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 10:1c42bbd596dc 136 MyEpos2.Reset();
joe_feubli 10:1c42bbd596dc 137
joe_feubli 10:1c42bbd596dc 138 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 139 MyEpos3.SetHomingPar(-3, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
joe_feubli 10:1c42bbd596dc 140 MyEpos3.Reset();
joe_feubli 10:1c42bbd596dc 141
joe_feubli 10:1c42bbd596dc 142 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 143 MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 10:1c42bbd596dc 144 MyEpos4.Reset();
joe_feubli 10:1c42bbd596dc 145
joe_feubli 10:1c42bbd596dc 146 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 147 MyEpos5.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
joe_feubli 10:1c42bbd596dc 148 MyEpos5.Reset();
joe_feubli 10:1c42bbd596dc 149
joe_feubli 10:1c42bbd596dc 150 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 151 MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT
joe_feubli 10:1c42bbd596dc 152 MyEpos6.Reset();
joe_feubli 10:1c42bbd596dc 153
joe_feubli 10:1c42bbd596dc 154 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 155 MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
joe_feubli 10:1c42bbd596dc 156 MyEpos7.Reset();
joe_feubli 10:1c42bbd596dc 157 */
ODEM 0:aaf583a75b97 158
joe_feubli 11:6e5ebe9aa84b 159 while(1){
joe_feubli 12:9e90d7a5a8c6 160
joe_feubli 12:9e90d7a5a8c6 161
joe_feubli 11:6e5ebe9aa84b 162 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 163 current_step = 7; //FEHLER
joe_feubli 12:9e90d7a5a8c6 164 ErrorState = MyEpos5.GetError();
joe_feubli 12:9e90d7a5a8c6 165 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 166 pc.printf("ErrorState: %d\r\n", ErrorState);
joe_feubli 11:6e5ebe9aa84b 167 }
joe_feubli 11:6e5ebe9aa84b 168
joe_feubli 2:0f4e77a22fd9 169 switch (current_step){
joe_feubli 2:0f4e77a22fd9 170 case 1://HOMING:
joe_feubli 12:9e90d7a5a8c6 171
joe_feubli 6:3eaa420dc2f5 172 led1 = 1;
joe_feubli 12:9e90d7a5a8c6 173 if (Power_ein == true){
joe_feubli 12:9e90d7a5a8c6 174 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 175 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 176 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 177 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 178 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 179 MyEpos6.Power(1);
joe_feubli 12:9e90d7a5a8c6 180 MyEpos7.Power(1);
joe_feubli 12:9e90d7a5a8c6 181 Power_ein = false;
joe_feubli 12:9e90d7a5a8c6 182 }
joe_feubli 6:3eaa420dc2f5 183
joe_feubli 6:3eaa420dc2f5 184 /***********************HOMING*************************/
joe_feubli 6:3eaa420dc2f5 185
joe_feubli 12:9e90d7a5a8c6 186
joe_feubli 7:44f2f9fd2eeb 187 if (node5_homed == false){
joe_feubli 12:9e90d7a5a8c6 188 pc.printf("Homing5\n");
joe_feubli 7:44f2f9fd2eeb 189 MyEpos5.Homing(); //Kopf
joe_feubli 12:9e90d7a5a8c6 190 //wait(5);
joe_feubli 12:9e90d7a5a8c6 191 MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 192 pc.printf("Fahren auf -60000\n");
joe_feubli 12:9e90d7a5a8c6 193 while(MyEpos5.TargetReached()!=1){
joe_feubli 12:9e90d7a5a8c6 194 }
joe_feubli 12:9e90d7a5a8c6 195 pc.printf("Target reached\n");
joe_feubli 7:44f2f9fd2eeb 196 node5_homed = true;
joe_feubli 7:44f2f9fd2eeb 197 break;
joe_feubli 7:44f2f9fd2eeb 198 }
joe_feubli 12:9e90d7a5a8c6 199
joe_feubli 6:3eaa420dc2f5 200 if (node1_homed == false){
joe_feubli 12:9e90d7a5a8c6 201 pc.printf("Homing1\n");
joe_feubli 6:3eaa420dc2f5 202 MyEpos1.Homing(); //Solarpanel_Aussen_Links
joe_feubli 6:3eaa420dc2f5 203 node1_homed = true;
joe_feubli 6:3eaa420dc2f5 204 break;
joe_feubli 6:3eaa420dc2f5 205 }
joe_feubli 12:9e90d7a5a8c6 206
joe_feubli 6:3eaa420dc2f5 207
joe_feubli 6:3eaa420dc2f5 208 if (node3_homed == false){
joe_feubli 12:9e90d7a5a8c6 209 pc.printf("Homing3\n");
joe_feubli 6:3eaa420dc2f5 210 MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
joe_feubli 6:3eaa420dc2f5 211 node3_homed = true;
joe_feubli 6:3eaa420dc2f5 212 break;
joe_feubli 6:3eaa420dc2f5 213 }
joe_feubli 12:9e90d7a5a8c6 214
joe_feubli 6:3eaa420dc2f5 215
joe_feubli 6:3eaa420dc2f5 216 if (node7_homed == false){
joe_feubli 12:9e90d7a5a8c6 217 pc.printf("Homing7\n");
joe_feubli 6:3eaa420dc2f5 218 MyEpos7.Homing(); //Bohrer
joe_feubli 6:3eaa420dc2f5 219 node7_homed = true;
joe_feubli 6:3eaa420dc2f5 220 break;
joe_feubli 6:3eaa420dc2f5 221 }
joe_feubli 6:3eaa420dc2f5 222
joe_feubli 12:9e90d7a5a8c6 223 if (node1_homed == true and node2_homed == false){
joe_feubli 12:9e90d7a5a8c6 224 pc.printf("Homing2\n");
joe_feubli 6:3eaa420dc2f5 225 MyEpos2.Homing(); //Solarpanel_Innen_Links
joe_feubli 6:3eaa420dc2f5 226 node2_homed = true;
joe_feubli 6:3eaa420dc2f5 227 break;
joe_feubli 2:0f4e77a22fd9 228 }
joe_feubli 6:3eaa420dc2f5 229
joe_feubli 6:3eaa420dc2f5 230 if (node3_homed == true and node4_homed == false){
joe_feubli 12:9e90d7a5a8c6 231 pc.printf("Homing4\n");
joe_feubli 6:3eaa420dc2f5 232 MyEpos4.Homing(); //Solarpanel_Innen_Rechts
joe_feubli 6:3eaa420dc2f5 233 node4_homed = true;
joe_feubli 12:9e90d7a5a8c6 234 wait(0.1);
joe_feubli 6:3eaa420dc2f5 235 break;
joe_feubli 6:3eaa420dc2f5 236 }
joe_feubli 12:9e90d7a5a8c6 237
joe_feubli 12:9e90d7a5a8c6 238
joe_feubli 6:3eaa420dc2f5 239
joe_feubli 6:3eaa420dc2f5 240 if (node7_homed == true and node6_homed == false){
joe_feubli 12:9e90d7a5a8c6 241 pc.printf("Homing6\n");
joe_feubli 6:3eaa420dc2f5 242 MyEpos6.Homing();//Bohrgestell
joe_feubli 6:3eaa420dc2f5 243 node6_homed = true;
joe_feubli 6:3eaa420dc2f5 244 break;
joe_feubli 6:3eaa420dc2f5 245 }
joe_feubli 12:9e90d7a5a8c6 246
joe_feubli 12:9e90d7a5a8c6 247 if (node2_homed==1 and node4_homed==1 and node5_grundposition == false){
joe_feubli 12:9e90d7a5a8c6 248 MyEpos5.MoveAbsolute(0,800,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 249 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 250 while(MyEpos5.TargetReached() != 1){
joe_feubli 12:9e90d7a5a8c6 251 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 252 }
joe_feubli 12:9e90d7a5a8c6 253 node5_grundposition = true;
joe_feubli 12:9e90d7a5a8c6 254 break;
joe_feubli 12:9e90d7a5a8c6 255 }
joe_feubli 12:9e90d7a5a8c6 256
joe_feubli 6:3eaa420dc2f5 257 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 258 pc.printf("Homing beendet\n");
joe_feubli 12:9e90d7a5a8c6 259 node5_grundposition == false;
joe_feubli 12:9e90d7a5a8c6 260 Power_ein == true;
joe_feubli 7:44f2f9fd2eeb 261 MyEpos1.Power(0);
joe_feubli 7:44f2f9fd2eeb 262 MyEpos2.Power(0);
joe_feubli 7:44f2f9fd2eeb 263 MyEpos3.Power(0);
joe_feubli 7:44f2f9fd2eeb 264 MyEpos4.Power(0);
joe_feubli 7:44f2f9fd2eeb 265 wait(0.1);
joe_feubli 7:44f2f9fd2eeb 266 MyEpos5.Power(0);
joe_feubli 11:6e5ebe9aa84b 267 current_step = 2;//BEREIT;
joe_feubli 2:0f4e77a22fd9 268 }
joe_feubli 12:9e90d7a5a8c6 269
joe_feubli 10:1c42bbd596dc 270
joe_feubli 10:1c42bbd596dc 271
joe_feubli 10:1c42bbd596dc 272
joe_feubli 8:98d4028cdea6 273
joe_feubli 6:3eaa420dc2f5 274 led1 = 0;
joe_feubli 2:0f4e77a22fd9 275 break;
joe_feubli 2:0f4e77a22fd9 276
joe_feubli 2:0f4e77a22fd9 277
joe_feubli 2:0f4e77a22fd9 278 case 2://BEREIT:
joe_feubli 12:9e90d7a5a8c6 279 //pc.printf("Bereit\n");
joe_feubli 6:3eaa420dc2f5 280 led2 = 1;
joe_feubli 8:98d4028cdea6 281 MyEpos5.SetDigOut(4,0);
joe_feubli 6:3eaa420dc2f5 282 if (MyEpos5.GetDigIn(3) == 1){
joe_feubli 8:98d4028cdea6 283 MyEpos5.SetDigOut(4,1);
joe_feubli 7:44f2f9fd2eeb 284 wait_ms(10);
joe_feubli 6:3eaa420dc2f5 285 MyEpos1.Power(1);
joe_feubli 6:3eaa420dc2f5 286 MyEpos2.Power(1);
joe_feubli 6:3eaa420dc2f5 287 MyEpos3.Power(1);
joe_feubli 11:6e5ebe9aa84b 288 MyEpos4.Power(1);
joe_feubli 6:3eaa420dc2f5 289 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 290 einfahren = false;
joe_feubli 11:6e5ebe9aa84b 291 current_step = 6;
joe_feubli 11:6e5ebe9aa84b 292 wait(0.5);
joe_feubli 6:3eaa420dc2f5 293 }
joe_feubli 11:6e5ebe9aa84b 294 wait_ms(10);
joe_feubli 9:039318b9096e 295 led2 = 0;
joe_feubli 2:0f4e77a22fd9 296 break;
ODEM 1:c53aafa72b36 297
joe_feubli 2:0f4e77a22fd9 298
joe_feubli 10:1c42bbd596dc 299 case 3://START_AUSFAHREN:
joe_feubli 10:1c42bbd596dc 300
joe_feubli 10:1c42bbd596dc 301 break;
joe_feubli 10:1c42bbd596dc 302
joe_feubli 10:1c42bbd596dc 303
joe_feubli 10:1c42bbd596dc 304 case 5://USV:
joe_feubli 12:9e90d7a5a8c6 305 pc.printf("USV\n");
joe_feubli 10:1c42bbd596dc 306 led4 = 1;
joe_feubli 10:1c42bbd596dc 307 MyEpos5.SetDigOut(4,1);
joe_feubli 10:1c42bbd596dc 308 switch (usv_step){
joe_feubli 10:1c42bbd596dc 309 case 1:
joe_feubli 12:9e90d7a5a8c6 310 // wait(0.1);
joe_feubli 11:6e5ebe9aa84b 311 // if (node1_homed == true){
joe_feubli 11:6e5ebe9aa84b 312 // MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links
joe_feubli 10:1c42bbd596dc 313 MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
joe_feubli 12:9e90d7a5a8c6 314 // wait(0.1);
joe_feubli 11:6e5ebe9aa84b 315 // }
joe_feubli 11:6e5ebe9aa84b 316 // if (node3_homed == true){
joe_feubli 11:6e5ebe9aa84b 317 // MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
joe_feubli 10:1c42bbd596dc 318 MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
joe_feubli 10:1c42bbd596dc 319 wait(0.1);
joe_feubli 11:6e5ebe9aa84b 320 // }
joe_feubli 10:1c42bbd596dc 321 /* if (node7_homed == true){
joe_feubli 10:1c42bbd596dc 322 MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 10:1c42bbd596dc 323 wait(0.1);
joe_feubli 10:1c42bbd596dc 324 }
joe_feubli 12:9e90d7a5a8c6 325 */// wait(0.1);
joe_feubli 11:6e5ebe9aa84b 326 // usv_step = 2;
joe_feubli 10:1c42bbd596dc 327 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 10:1c42bbd596dc 328 usv_step = 2;
joe_feubli 12:9e90d7a5a8c6 329 wait(0.1);
joe_feubli 10:1c42bbd596dc 330 }
joe_feubli 10:1c42bbd596dc 331 break;
joe_feubli 10:1c42bbd596dc 332
joe_feubli 10:1c42bbd596dc 333 case 2:
joe_feubli 12:9e90d7a5a8c6 334 // if (MyEpos1.TargetReached()==1){
joe_feubli 12:9e90d7a5a8c6 335 // wait(1);
joe_feubli 10:1c42bbd596dc 336 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 10:1c42bbd596dc 337 MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 12:9e90d7a5a8c6 338 // wait_ms(10);
joe_feubli 12:9e90d7a5a8c6 339 // }
joe_feubli 12:9e90d7a5a8c6 340 // if (MyEpos3.TargetReached()==1){
joe_feubli 12:9e90d7a5a8c6 341 // wait(1);
joe_feubli 11:6e5ebe9aa84b 342 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 11:6e5ebe9aa84b 343 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 12:9e90d7a5a8c6 344 // wait_ms(10);
joe_feubli 12:9e90d7a5a8c6 345 // }
joe_feubli 12:9e90d7a5a8c6 346 // wait(0.1);
joe_feubli 10:1c42bbd596dc 347 /* if (node4_homed == true and MyEpos3.TargetReached()==1){
joe_feubli 10:1c42bbd596dc 348 wait(1);
joe_feubli 10:1c42bbd596dc 349 MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
joe_feubli 10:1c42bbd596dc 350 wait_ms(10);
joe_feubli 10:1c42bbd596dc 351 }
joe_feubli 10:1c42bbd596dc 352 if (node6_homed == true and MyEpos7.TargetReached()==1){
joe_feubli 10:1c42bbd596dc 353 wait(1);
joe_feubli 10:1c42bbd596dc 354 MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer
joe_feubli 10:1c42bbd596dc 355 wait_ms(10);
joe_feubli 10:1c42bbd596dc 356 }
joe_feubli 12:9e90d7a5a8c6 357 */ wait(0.1);
joe_feubli 11:6e5ebe9aa84b 358 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
joe_feubli 10:1c42bbd596dc 359 usv_step = 3;
joe_feubli 12:9e90d7a5a8c6 360 wait(0.1);
joe_feubli 10:1c42bbd596dc 361 }
joe_feubli 10:1c42bbd596dc 362 break;
joe_feubli 10:1c42bbd596dc 363
joe_feubli 10:1c42bbd596dc 364 case 3:
joe_feubli 10:1c42bbd596dc 365 MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
joe_feubli 10:1c42bbd596dc 366 wait(0.1);
joe_feubli 10:1c42bbd596dc 367 if (MyEpos5.TargetReached()==1){
joe_feubli 10:1c42bbd596dc 368 usv_step = 4;
joe_feubli 10:1c42bbd596dc 369 wait(0.1);
joe_feubli 10:1c42bbd596dc 370 }
joe_feubli 10:1c42bbd596dc 371 break;
joe_feubli 10:1c42bbd596dc 372
joe_feubli 10:1c42bbd596dc 373 case 4:
joe_feubli 10:1c42bbd596dc 374 /*while (MyEpos5.TargetReached()==0){
joe_feubli 10:1c42bbd596dc 375 wait_ms(10);
joe_feubli 10:1c42bbd596dc 376 }*/
joe_feubli 12:9e90d7a5a8c6 377 /* MyEpos1.Power(0);
joe_feubli 10:1c42bbd596dc 378 MyEpos2.Power(0);
joe_feubli 10:1c42bbd596dc 379 MyEpos3.Power(0);
joe_feubli 10:1c42bbd596dc 380 MyEpos4.Power(0);
joe_feubli 10:1c42bbd596dc 381 MyEpos5.Power(0);
joe_feubli 10:1c42bbd596dc 382 MyEpos6.Power(0);
joe_feubli 10:1c42bbd596dc 383 MyEpos7.Power(0);
joe_feubli 10:1c42bbd596dc 384 usv_step = 5;
joe_feubli 12:9e90d7a5a8c6 385 */ MyEpos1.Power(0);
joe_feubli 10:1c42bbd596dc 386 MyEpos2.Power(0);
joe_feubli 10:1c42bbd596dc 387 MyEpos3.Power(0);
joe_feubli 10:1c42bbd596dc 388 MyEpos4.Power(0);
joe_feubli 10:1c42bbd596dc 389 MyEpos5.Power(0);
joe_feubli 10:1c42bbd596dc 390 MyEpos6.Power(0);
joe_feubli 10:1c42bbd596dc 391 MyEpos7.Power(0);
joe_feubli 10:1c42bbd596dc 392 usv_step = 5;
joe_feubli 12:9e90d7a5a8c6 393 wait(0.1);
joe_feubli 10:1c42bbd596dc 394 break;
joe_feubli 10:1c42bbd596dc 395
joe_feubli 10:1c42bbd596dc 396 case 5:
joe_feubli 10:1c42bbd596dc 397 if(buffering == 1){
joe_feubli 10:1c42bbd596dc 398 led3 = 1;
joe_feubli 10:1c42bbd596dc 399 } else {
joe_feubli 10:1c42bbd596dc 400 led3 = 0;
joe_feubli 10:1c42bbd596dc 401 current_step=1;
joe_feubli 10:1c42bbd596dc 402 usv_step=1;
joe_feubli 10:1c42bbd596dc 403 ausfahren_step=1;
joe_feubli 10:1c42bbd596dc 404 einfahren_step=1;
joe_feubli 10:1c42bbd596dc 405 }
joe_feubli 10:1c42bbd596dc 406 /*if (buffering == 0){
joe_feubli 10:1c42bbd596dc 407 current_step = 1;
joe_feubli 10:1c42bbd596dc 408 usv_step = 1;
joe_feubli 10:1c42bbd596dc 409 }*/
joe_feubli 10:1c42bbd596dc 410 wait(1);
joe_feubli 10:1c42bbd596dc 411 break;
joe_feubli 10:1c42bbd596dc 412 }
joe_feubli 10:1c42bbd596dc 413 led4 = 0;
joe_feubli 10:1c42bbd596dc 414 break;
joe_feubli 10:1c42bbd596dc 415
joe_feubli 10:1c42bbd596dc 416 case 6://Test
joe_feubli 12:9e90d7a5a8c6 417 // pc.printf("Läuft...\n");
joe_feubli 7:44f2f9fd2eeb 418 led3 = 1;
joe_feubli 6:3eaa420dc2f5 419 if (einfahren == false){
joe_feubli 6:3eaa420dc2f5 420 switch (ausfahren_step){
joe_feubli 6:3eaa420dc2f5 421 case 1:
joe_feubli 6:3eaa420dc2f5 422 MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
joe_feubli 6:3eaa420dc2f5 423 ausfahren_step = 2;
joe_feubli 6:3eaa420dc2f5 424 wait(1);
joe_feubli 6:3eaa420dc2f5 425 break;
joe_feubli 6:3eaa420dc2f5 426
joe_feubli 6:3eaa420dc2f5 427 case 2:
joe_feubli 6:3eaa420dc2f5 428 if (MyEpos5.TargetReached()==1){
joe_feubli 7:44f2f9fd2eeb 429 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 7:44f2f9fd2eeb 430 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 11:6e5ebe9aa84b 431 MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Innen Links
joe_feubli 11:6e5ebe9aa84b 432 MyEpos4.MoveAbsolute(-310000,500,100,100); //Panel Innen Rechts
joe_feubli 6:3eaa420dc2f5 433 ausfahren_step = 3;
joe_feubli 10:1c42bbd596dc 434 wait(1);
joe_feubli 6:3eaa420dc2f5 435 }
joe_feubli 6:3eaa420dc2f5 436 break;
joe_feubli 6:3eaa420dc2f5 437
joe_feubli 6:3eaa420dc2f5 438 case 3:
joe_feubli 11:6e5ebe9aa84b 439 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 440 MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
joe_feubli 6:3eaa420dc2f5 441 MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 442
joe_feubli 6:3eaa420dc2f5 443 ausfahren_step = 4;
joe_feubli 6:3eaa420dc2f5 444 wait(1);
joe_feubli 6:3eaa420dc2f5 445 }
joe_feubli 6:3eaa420dc2f5 446 break;
joe_feubli 6:3eaa420dc2f5 447
joe_feubli 6:3eaa420dc2f5 448 case 4:
joe_feubli 6:3eaa420dc2f5 449 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 450 einfahren = true;
joe_feubli 6:3eaa420dc2f5 451 einfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 452 wait(1);
joe_feubli 6:3eaa420dc2f5 453 }
joe_feubli 6:3eaa420dc2f5 454 break;
joe_feubli 12:9e90d7a5a8c6 455
joe_feubli 12:9e90d7a5a8c6 456 }//switch (ausfahren_step)
joe_feubli 12:9e90d7a5a8c6 457 }//if (einfahren == false)
joe_feubli 6:3eaa420dc2f5 458
joe_feubli 6:3eaa420dc2f5 459 if (einfahren == true){
joe_feubli 6:3eaa420dc2f5 460 ausfahren_step = 1;
joe_feubli 6:3eaa420dc2f5 461 switch (einfahren_step){
joe_feubli 6:3eaa420dc2f5 462 case 1:
joe_feubli 11:6e5ebe9aa84b 463 /* MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links
joe_feubli 10:1c42bbd596dc 464 MyEpos3.MoveAbsolute(-1000,500,100,100); //Panel Aussen Rechts
joe_feubli 11:6e5ebe9aa84b 465 */ MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 466 MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
joe_feubli 6:3eaa420dc2f5 467 einfahren_step = 2;
joe_feubli 12:9e90d7a5a8c6 468 wait(0.1);
joe_feubli 6:3eaa420dc2f5 469 break;
joe_feubli 6:3eaa420dc2f5 470
joe_feubli 6:3eaa420dc2f5 471 case 2:
joe_feubli 6:3eaa420dc2f5 472 if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
joe_feubli 12:9e90d7a5a8c6 473 // wait(0.1);
joe_feubli 8:98d4028cdea6 474 MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
joe_feubli 8:98d4028cdea6 475 MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
joe_feubli 11:6e5ebe9aa84b 476 MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
joe_feubli 11:6e5ebe9aa84b 477 MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
joe_feubli 6:3eaa420dc2f5 478 einfahren_step = 3;
joe_feubli 12:9e90d7a5a8c6 479 wait(0.1);
joe_feubli 6:3eaa420dc2f5 480 }
joe_feubli 6:3eaa420dc2f5 481 break;
joe_feubli 6:3eaa420dc2f5 482
joe_feubli 6:3eaa420dc2f5 483 case 3:
joe_feubli 11:6e5ebe9aa84b 484 if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 485 MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
joe_feubli 12:9e90d7a5a8c6 486 // wait(1);
joe_feubli 6:3eaa420dc2f5 487 MyEpos1.Power(0);
joe_feubli 6:3eaa420dc2f5 488 MyEpos2.Power(0);
joe_feubli 6:3eaa420dc2f5 489 MyEpos3.Power(0);
joe_feubli 6:3eaa420dc2f5 490 MyEpos4.Power(0);
joe_feubli 6:3eaa420dc2f5 491 einfahren_step = 4;
joe_feubli 12:9e90d7a5a8c6 492 wait(0.1);
joe_feubli 6:3eaa420dc2f5 493 }
joe_feubli 6:3eaa420dc2f5 494 break;
joe_feubli 6:3eaa420dc2f5 495
joe_feubli 6:3eaa420dc2f5 496 case 4:
joe_feubli 6:3eaa420dc2f5 497 if (MyEpos5.TargetReached()==1){
joe_feubli 6:3eaa420dc2f5 498 einfahren = false;
joe_feubli 6:3eaa420dc2f5 499 current_step = 2;
joe_feubli 6:3eaa420dc2f5 500 wait(1);
joe_feubli 6:3eaa420dc2f5 501 MyEpos5.Power(0);
joe_feubli 6:3eaa420dc2f5 502 }
joe_feubli 10:1c42bbd596dc 503 break;
joe_feubli 10:1c42bbd596dc 504
joe_feubli 12:9e90d7a5a8c6 505 }//switch(einfahren_step)
joe_feubli 12:9e90d7a5a8c6 506 }//if einfahren == true)
joe_feubli 7:44f2f9fd2eeb 507 led3 = 0;
ODEM 1:c53aafa72b36 508 break;
joe_feubli 12:9e90d7a5a8c6 509
joe_feubli 12:9e90d7a5a8c6 510 case 7:
joe_feubli 12:9e90d7a5a8c6 511 pc.printf("Fehler RESET\n");
joe_feubli 12:9e90d7a5a8c6 512 // if(MyEpos5.GetDigIn(3) == 1){
joe_feubli 12:9e90d7a5a8c6 513 MyEpos1.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 514 MyEpos2.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 515 MyEpos3.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 516 MyEpos4.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 517 MyEpos5.MoveVelocity(0, 500, 500);
joe_feubli 12:9e90d7a5a8c6 518 wait(3);
joe_feubli 12:9e90d7a5a8c6 519 MyEpos1.Reset();
joe_feubli 12:9e90d7a5a8c6 520 MyEpos1.Power(1);
joe_feubli 12:9e90d7a5a8c6 521 MyEpos2.Reset();
joe_feubli 12:9e90d7a5a8c6 522 MyEpos2.Power(1);
joe_feubli 12:9e90d7a5a8c6 523 MyEpos3.Reset();
joe_feubli 12:9e90d7a5a8c6 524 MyEpos3.Power(1);
joe_feubli 12:9e90d7a5a8c6 525 MyEpos4.Reset();
joe_feubli 12:9e90d7a5a8c6 526 MyEpos4.Power(1);
joe_feubli 12:9e90d7a5a8c6 527 MyEpos5.Reset();
joe_feubli 12:9e90d7a5a8c6 528 MyEpos5.Power(1);
joe_feubli 12:9e90d7a5a8c6 529 node1_homed = false;
joe_feubli 12:9e90d7a5a8c6 530 node2_homed = false;
joe_feubli 12:9e90d7a5a8c6 531 node3_homed = false;
joe_feubli 12:9e90d7a5a8c6 532 node4_homed = false;
joe_feubli 12:9e90d7a5a8c6 533 node5_homed = false;
joe_feubli 12:9e90d7a5a8c6 534 current_step = 1;
joe_feubli 12:9e90d7a5a8c6 535 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 536 // }
joe_feubli 12:9e90d7a5a8c6 537
joe_feubli 12:9e90d7a5a8c6 538 // MyEpos5.MoveVelocity(0,500,500);
joe_feubli 12:9e90d7a5a8c6 539 wait(0.1);
joe_feubli 12:9e90d7a5a8c6 540
joe_feubli 12:9e90d7a5a8c6 541 /* MyEpos1.Reset();
joe_feubli 12:9e90d7a5a8c6 542 MyEpos2.Reset();
joe_feubli 12:9e90d7a5a8c6 543 MyEpos3.Reset();
joe_feubli 12:9e90d7a5a8c6 544 MyEpos4.Reset();
joe_feubli 12:9e90d7a5a8c6 545 MyEpos5.Reset();
joe_feubli 12:9e90d7a5a8c6 546 wait(0.5);
joe_feubli 12:9e90d7a5a8c6 547 */
joe_feubli 12:9e90d7a5a8c6 548
joe_feubli 12:9e90d7a5a8c6 549 break;
joe_feubli 12:9e90d7a5a8c6 550 }//switch (current_step)
joe_feubli 2:0f4e77a22fd9 551 }//while
joe_feubli 10:1c42bbd596dc 552 }//main