Programm mit Ablauf
Dependencies: EPOS2 mbed-rtos mbed
Fork of Projekt_Kugelbahn by
main.cpp
- Committer:
- joe_feubli
- Date:
- 2016-10-18
- Revision:
- 12:9e90d7a5a8c6
- Parent:
- 11:6e5ebe9aa84b
- Child:
- 13:cc43e546f310
File content as of revision 12:9e90d7a5a8c6:
#include "mbed.h" #include "EPOS2.h" //***********************************************************************************************************************************// //*** Global_Var *******************************************************************************************************************// //***********************************************************************************************************************************// Serial pc(USBTX, USBRX); // (tx, rx) CAN can(p9, p10); // Can Pin def /* int velocity = 3000; int acceleration = 50000; int deceleration = 50000; int anzahl_zyklen = 1000000; int absolvierte_zyklen = 0; int ActualPos = 0; int DemandPos = 0; int ActualCurrent = 0; int DemandCurrent = 0; int counter=0; int counter2=0; bool start = true; int counter_s = false; int counter_s2 = false; */ DigitalIn usv_ready(p23); InterruptIn buffering(p24); DigitalIn replace_battery(p25); DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); bool node1_homed = false; bool node2_homed = false; bool node3_homed = false; bool node4_homed = false; bool node5_homed = false; bool node6_homed = true; bool node7_homed = true; bool Power_ein = true; bool node5_grundposition = false; int homing = false; int ready = false; int ausfahren = false; int einfahren = false; int usv = false; int current_step = 1; //HOMING int ausfahren_step = 1; int einfahren_step = 1; int usv_step = 1; int ErrorState; void interrupt_usv(){ current_step = 5; wait_ms(10); } int main(){ ///////// Main led1 = 1; pc.baud(9600); buffering.rise(&interrupt_usv); //Interrupt für USV wait(0.5); //Wartezeit bis Epos2 24/5 aufgestartet ist led2 = 1; wait(0.5); led3 = 1; wait(0.5); led4 = 1; wait(0.5); pc.printf("Initialisation CAN\n"); can.frequency(1000000); //Define Can baud in bit/s CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec) canOpen.start(); //Start defined CanOpen Network EPOS2 MyEpos1(&canOpen, 1); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) EPOS2 MyEpos3(&canOpen, 3); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) EPOS2 MyEpos4(&canOpen, 4); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) EPOS2 MyEpos5(&canOpen, 5); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) EPOS2 MyEpos6(&canOpen, 6); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) EPOS2 MyEpos7(&canOpen, 7); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) pc.printf("Initialisation abgeschlossen\n"); wait(0.1); MyEpos1.Reset(); MyEpos2.Reset(); MyEpos3.Reset(); MyEpos4.Reset(); MyEpos5.Reset(); MyEpos6.Reset(); MyEpos7.Reset(); wait(0.1); led1 = 0; led2 = 0; led3 = 0; led4 = 0; MyEpos5.SetDigOut(4, 1); /* usv_ready.mode(PullUp); buffering.mode(PullUp); replace_battery.mode(PullUp); */ //************** Initialisierung **************// /* HOMING METHODEN: Homing Method 7 :Home Switch Positive Speed & Index Homing Method 11:Home Switch Negative Speed & Index Homing Method 23: Home Switch Positive Speed Homing Method 27: Home Switch Negative Speed Homing Method -3: Current Threshold Positive Speed Homing Method -4: Current Threshold Negative Speed */ //Motortype 10: EC-Motor /* MyEpos1.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT MyEpos1.SetHomingPar(-4, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT MyEpos1.Reset(); MyEpos2.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT MyEpos2.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset) MyEpos2.Reset(); MyEpos3.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT MyEpos3.SetHomingPar(-3, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT MyEpos3.Reset(); MyEpos4.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT MyEpos4.Reset(); MyEpos5.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT MyEpos5.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT MyEpos5.Reset(); MyEpos6.SetPar(10,3440, 1, 12000, 3.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT MyEpos6.Reset(); MyEpos7.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding) MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset) MyEpos7.Reset(); */ while(1){ 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*/){ current_step = 7; //FEHLER ErrorState = MyEpos5.GetError(); wait(0.1); pc.printf("ErrorState: %d\r\n", ErrorState); } switch (current_step){ case 1://HOMING: led1 = 1; if (Power_ein == true){ MyEpos1.Power(1); MyEpos2.Power(1); MyEpos3.Power(1); MyEpos4.Power(1); MyEpos5.Power(1); MyEpos6.Power(1); MyEpos7.Power(1); Power_ein = false; } /***********************HOMING*************************/ if (node5_homed == false){ pc.printf("Homing5\n"); MyEpos5.Homing(); //Kopf //wait(5); MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf pc.printf("Fahren auf -60000\n"); while(MyEpos5.TargetReached()!=1){ } pc.printf("Target reached\n"); node5_homed = true; break; } if (node1_homed == false){ pc.printf("Homing1\n"); MyEpos1.Homing(); //Solarpanel_Aussen_Links node1_homed = true; break; } if (node3_homed == false){ pc.printf("Homing3\n"); MyEpos3.Homing(); //Solarpanel_Aussen_Rechts node3_homed = true; break; } if (node7_homed == false){ pc.printf("Homing7\n"); MyEpos7.Homing(); //Bohrer node7_homed = true; break; } if (node1_homed == true and node2_homed == false){ pc.printf("Homing2\n"); MyEpos2.Homing(); //Solarpanel_Innen_Links node2_homed = true; break; } if (node3_homed == true and node4_homed == false){ pc.printf("Homing4\n"); MyEpos4.Homing(); //Solarpanel_Innen_Rechts node4_homed = true; wait(0.1); break; } if (node7_homed == true and node6_homed == false){ pc.printf("Homing6\n"); MyEpos6.Homing();//Bohrgestell node6_homed = true; break; } if (node2_homed==1 and node4_homed==1 and node5_grundposition == false){ MyEpos5.MoveAbsolute(0,800,100,100); //Kopf wait(0.1); while(MyEpos5.TargetReached() != 1){ wait(0.1); } node5_grundposition = true; break; } 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){ pc.printf("Homing beendet\n"); node5_grundposition == false; Power_ein == true; MyEpos1.Power(0); MyEpos2.Power(0); MyEpos3.Power(0); MyEpos4.Power(0); wait(0.1); MyEpos5.Power(0); current_step = 2;//BEREIT; } led1 = 0; break; case 2://BEREIT: //pc.printf("Bereit\n"); led2 = 1; MyEpos5.SetDigOut(4,0); if (MyEpos5.GetDigIn(3) == 1){ MyEpos5.SetDigOut(4,1); wait_ms(10); MyEpos1.Power(1); MyEpos2.Power(1); MyEpos3.Power(1); MyEpos4.Power(1); MyEpos5.Power(1); einfahren = false; current_step = 6; wait(0.5); } wait_ms(10); led2 = 0; break; case 3://START_AUSFAHREN: break; case 5://USV: pc.printf("USV\n"); led4 = 1; MyEpos5.SetDigOut(4,1); switch (usv_step){ case 1: // wait(0.1); // if (node1_homed == true){ // MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links // wait(0.1); // } // if (node3_homed == true){ // MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts wait(0.1); // } /* if (node7_homed == true){ MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer wait(0.1); } */// wait(0.1); // usv_step = 2; if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){ usv_step = 2; wait(0.1); } break; case 2: // if (MyEpos1.TargetReached()==1){ // wait(1); MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links // wait_ms(10); // } // if (MyEpos3.TargetReached()==1){ // wait(1); MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links // wait_ms(10); // } // wait(0.1); /* if (node4_homed == true and MyEpos3.TargetReached()==1){ wait(1); MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links wait_ms(10); } if (node6_homed == true and MyEpos7.TargetReached()==1){ wait(1); MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer wait_ms(10); } */ wait(0.1); if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){ usv_step = 3; wait(0.1); } break; case 3: MyEpos5.MoveAbsolute(0,800,500,500); //Kopf wait(0.1); if (MyEpos5.TargetReached()==1){ usv_step = 4; wait(0.1); } break; case 4: /*while (MyEpos5.TargetReached()==0){ wait_ms(10); }*/ /* MyEpos1.Power(0); MyEpos2.Power(0); MyEpos3.Power(0); MyEpos4.Power(0); MyEpos5.Power(0); MyEpos6.Power(0); MyEpos7.Power(0); usv_step = 5; */ MyEpos1.Power(0); MyEpos2.Power(0); MyEpos3.Power(0); MyEpos4.Power(0); MyEpos5.Power(0); MyEpos6.Power(0); MyEpos7.Power(0); usv_step = 5; wait(0.1); break; case 5: if(buffering == 1){ led3 = 1; } else { led3 = 0; current_step=1; usv_step=1; ausfahren_step=1; einfahren_step=1; } /*if (buffering == 0){ current_step = 1; usv_step = 1; }*/ wait(1); break; } led4 = 0; break; case 6://Test // pc.printf("Läuft...\n"); led3 = 1; if (einfahren == false){ switch (ausfahren_step){ case 1: MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf ausfahren_step = 2; wait(1); break; case 2: if (MyEpos5.TargetReached()==1){ MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Innen Links MyEpos4.MoveAbsolute(-310000,500,100,100); //Panel Innen Rechts ausfahren_step = 3; wait(1); } break; case 3: if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){ MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts ausfahren_step = 4; wait(1); } break; case 4: if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){ einfahren = true; einfahren_step = 1; wait(1); } break; }//switch (ausfahren_step) }//if (einfahren == false) if (einfahren == true){ ausfahren_step = 1; switch (einfahren_step){ case 1: /* MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links MyEpos3.MoveAbsolute(-1000,500,100,100); //Panel Aussen Rechts */ MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts einfahren_step = 2; wait(0.1); break; case 2: if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){ // wait(0.1); MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts einfahren_step = 3; wait(0.1); } break; case 3: if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){ MyEpos5.MoveAbsolute(0,500,100,100); //Kopf // wait(1); MyEpos1.Power(0); MyEpos2.Power(0); MyEpos3.Power(0); MyEpos4.Power(0); einfahren_step = 4; wait(0.1); } break; case 4: if (MyEpos5.TargetReached()==1){ einfahren = false; current_step = 2; wait(1); MyEpos5.Power(0); } break; }//switch(einfahren_step) }//if einfahren == true) led3 = 0; break; case 7: pc.printf("Fehler RESET\n"); // if(MyEpos5.GetDigIn(3) == 1){ MyEpos1.MoveVelocity(0, 500, 500); MyEpos2.MoveVelocity(0, 500, 500); MyEpos3.MoveVelocity(0, 500, 500); MyEpos4.MoveVelocity(0, 500, 500); MyEpos5.MoveVelocity(0, 500, 500); wait(3); MyEpos1.Reset(); MyEpos1.Power(1); MyEpos2.Reset(); MyEpos2.Power(1); MyEpos3.Reset(); MyEpos3.Power(1); MyEpos4.Reset(); MyEpos4.Power(1); MyEpos5.Reset(); MyEpos5.Power(1); node1_homed = false; node2_homed = false; node3_homed = false; node4_homed = false; node5_homed = false; current_step = 1; wait(0.1); // } // MyEpos5.MoveVelocity(0,500,500); wait(0.1); /* MyEpos1.Reset(); MyEpos2.Reset(); MyEpos3.Reset(); MyEpos4.Reset(); MyEpos5.Reset(); wait(0.5); */ break; }//switch (current_step) }//while }//main