Programm mit Ablauf
Dependencies: EPOS2 mbed-rtos mbed
Fork of Projekt_Kugelbahn by
main.cpp
- Committer:
- joe_feubli
- Date:
- 2016-10-17
- Revision:
- 11:6e5ebe9aa84b
- Parent:
- 10:1c42bbd596dc
- Child:
- 12:9e90d7a5a8c6
File content as of revision 11:6e5ebe9aa84b:
#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; 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; void interrupt_usv(){ current_step = 5; wait_ms(10); } int main(){ ///////// Main led1 = 1; pc.baud(9600); buffering.rise(&interrupt_usv); buffering.rise(&interrupt_usv); //Interrupt für USV wait(0.5); //Wartezeit bis Epos2 70/10 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"); 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 } switch (current_step){ case 1://HOMING: led1 = 1; MyEpos1.Power(1); MyEpos2.Power(1); MyEpos3.Power(1); MyEpos4.Power(1); MyEpos5.Power(1); MyEpos6.Power(1); MyEpos7.Power(1); /***********************HOMING*************************/ if (node5_homed == false){ MyEpos5.Homing(); //Kopf node5_homed = true; break; } if (node1_homed == false){ MyEpos1.Homing(); //Solarpanel_Aussen_Links node1_homed = true; break; } if (node3_homed == false){ MyEpos3.Homing(); //Solarpanel_Aussen_Rechts node3_homed = true; break; } if (node7_homed == false){ MyEpos7.Homing(); //Bohrer node7_homed = true; break; } if (node1_homed == true and node2_homed == false and node5_homed == true){ MyEpos5.MoveAbsolute(-3000,500,100,100); //Kopf MyEpos2.Homing(); //Solarpanel_Innen_Links MyEpos5.MoveAbsolute(500,100,50,50); //Kopf wait(2); node2_homed = true; break; } if (node3_homed == true and node4_homed == false){ MyEpos4.Homing(); //Solarpanel_Innen_Rechts node4_homed = true; break; } if (node7_homed == true and node6_homed == false){ MyEpos6.Homing();//Bohrgestell node6_homed = 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){ 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: 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); current_step = 6; wait(0.5); } wait_ms(10); led2 = 0; break; case 3://START_AUSFAHREN: break; case 5://USV: 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; } 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(1); if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){ usv_step = 3; } 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; 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 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; } } 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(1); break; case 2: if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){ wait(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(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; } break; case 4: if (MyEpos5.TargetReached()==1){ einfahren = false; current_step = 2; wait(1); MyEpos5.Power(0); } break; } } led3 = 0; break; }//case }//while }//main