2 Achsen Motorsteuerung mit Fehlerauswertung
Dependencies: EPOS2 mbed-rtos mbed
main.cpp
- Committer:
- ODEM
- Date:
- 2016-07-28
- Revision:
- 1:c53aafa72b36
- Parent:
- 0:aaf583a75b97
File content as of revision 1:c53aafa72b36:
#include "mbed.h" #include "EPOS2.h" //#include "RobotControl.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 current_State = 2; int counter_s = false; int counter_s2 = false; void Motor1 (EPOS2 MyEpos){ pc.printf("Motor1\n"); MyEpos.SetDigOut(3, 0); wait(2); // while(1){ // pc.printf("start\n"); MyEpos.SetDigOut(3, 1); //(1=Enable, 0=Disable) MyEpos.MoveAbsolute(500,20,500,deceleration); MyEpos.SetDigOut(3, 0); //(TargetPosition, PosSpeed, acceleration, deceleration) wait(1); // pc.printf("%d\n", MyEpos.GetError()); while (MyEpos.TargetReached() == 0) { ActualPos = MyEpos.GetActualPos(); DemandPos = MyEpos.GetDemandPos(); ActualCurrent = MyEpos.GetActualCurrentAveraged(); DemandCurrent = MyEpos.GetDemandCurrent(); //wait_ms(100); MyEpos.SetDigOut(3, 0); } // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent); wait(0.5); MyEpos.SetDigOut(3, 1); //wait_ms(100); MyEpos.MoveAbsolute(0,3000,3000,3000); MyEpos.SetDigOut(3, 0); wait(1); while (MyEpos.TargetReached() == 0) { ActualPos = MyEpos.GetActualPos(); DemandPos = MyEpos.GetDemandPos(); ActualCurrent = MyEpos.GetActualCurrent(); DemandCurrent = MyEpos.GetDemandCurrent(); //wait_ms(100); MyEpos.SetDigOut(3, 0); //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent); } counter=0; //} } void Motor2(EPOS2 MyEpos2){ pc.printf("Motor2\n"); wait_ms(100) ; MyEpos2. MoveVelocity(1000, 80, 1000); wait(0.1); } int main() ///////// Main { pc.baud(9600); wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist 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 MyEpos(&canOpen, 1); EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) pc.printf("Initialisation abgeschlossen\n"); while(1){ //if(counter>2){ // MyEpos.Power(0); // MyEpos2.Power(0); // } switch (current_State){ case 1://STATE_NORMAL: if (MyEpos.GetError()==true && counter<3) { pc.printf("FEHLER-Node1!!!\n"); current_State = 3;//ERROR; } if (MyEpos2.GetError()==true&& counter2<3) { pc.printf("FEHLER-Node2!!!\n"); current_State = 5;//ERROR; } if (counter2>2){ MyEpos2.Power(0); MyEpos.Power(0); } if (counter>2){ MyEpos2.Power(0); MyEpos.Power(0); } Motor2(MyEpos2); Motor1(MyEpos); break; case 2:// INITIALISIERUNG: //************** Initialisierung **************// counter_s=false; counter_s2=false; pc.printf("Fehlerfall %d\n", counter) ; pc.printf("Fehlerfall2 %d\n", counter2) ; MyEpos.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding) MyEpos.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset) MyEpos.Reset(); //Reset the EPOS2 wait(2); MyEpos2.SetPar(10,936, 8, 1000, 29.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding) MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset) MyEpos2.Reset(); //Reset the EPOS2 wait(2); pc.printf("Homing\n"); MyEpos.Power(1); pc.printf("power\n"); MyEpos2.Power(1); //Enable the Epos for Homing pc.printf("Power2\n"); wait(2); MyEpos.Homing(); } //do Homing and wait untill homing is done // MyEpos.Power(1); wait_ms(100); current_State = 1;//STATE_NORMAL; break; case 3://ERROR1: if (counter_s==false){ counter++; counter_s=true; current_State = 2;// INITIALISIERUNG; } break; case 4: //STATE_OFF: MyEpos.Power(0); MyEpos2.Power(0); break; case 5://ERROR2 if (counter_s2==false){ counter2++; counter_s2=true; current_State = 2;// INITIALISIERUNG; } break; } // pc.printf("%d\n", MyEpos.GetError()); }//while }//main