From 7b66c5e1238ce5b45c40fed393fc3ac119b16f4a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 9 Oct 2010 13:53:55 +0200 Subject: add absolute position control --- AT91SAM7S256/Source/c_output.c | 10 +++--- AT91SAM7S256/Source/c_output.iom | 7 ++-- AT91SAM7S256/Source/d_output.c | 69 ++++++++++++++++++++++++++++++++++++++-- AT91SAM7S256/Source/d_output.h | 2 ++ 4 files changed, 78 insertions(+), 10 deletions(-) diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index 4601ff3..2d40f06 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -93,11 +93,6 @@ void cOutputCtrl(void) dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter); } } - if (pOut->Flags & UPDATE_TACHO_LIMIT) - { - pOut->Flags &= ~UPDATE_TACHO_LIMIT; - dOutputSetTachoLimit(Tmp, pOut->TachoLimit); - } if (pOut->Flags & UPDATE_MODE) { pOut->Flags &= ~UPDATE_MODE; @@ -128,6 +123,11 @@ void cOutputCtrl(void) dOutputDisableRegulation(Tmp); } } + if (pOut->Flags & UPDATE_TACHO_LIMIT) + { + pOut->Flags &= ~UPDATE_TACHO_LIMIT; + dOutputSetTachoLimit(Tmp, pOut->TachoLimit); + } if (pOut->Flags & UPDATE_PID_VALUES) { pOut->Flags &= ~UPDATE_PID_VALUES; diff --git a/AT91SAM7S256/Source/c_output.iom b/AT91SAM7S256/Source/c_output.iom index fc26efd..794f7de 100644 --- a/AT91SAM7S256/Source/c_output.iom +++ b/AT91SAM7S256/Source/c_output.iom @@ -49,9 +49,10 @@ enum // Constant related to RegMode enum { - REGULATION_MODE_IDLE, - REGULATION_MODE_MOTOR_SPEED, - REGULATION_MODE_MOTOR_SYNC + REGULATION_MODE_IDLE = 0, + REGULATION_MODE_MOTOR_SPEED = 1, + REGULATION_MODE_MOTOR_SYNC = 2, + REGULATION_MODE_MOTOR_POS = 4, }; typedef struct diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index 3608cb7..44a43a7 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -278,7 +278,12 @@ void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewReg void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel) { MOTORDATA * pMD = &(MotorData[MotorNr]); - if (BlockTachoCntToTravel == 0) + if (pMD->RegulationMode & REGSTATE_POSITION) + { + pMD->MotorRunForever = 0; + pMD->MotorTachoCountToRun = BlockTachoCntToTravel; + } + else if (BlockTachoCntToTravel == 0) { pMD->MotorRunForever = 1; } @@ -664,6 +669,11 @@ void dOutputRampDownFunction(UBYTE MotorNr) void dOutputTachoLimitControl(UBYTE MotorNr) { MOTORDATA * pMD = &(MotorData[MotorNr]); + if (pMD->RegulationMode & REGSTATE_POSITION) + { + /* No limit when doing absolute position regulation. */ + return; + } if (pMD->MotorRunForever == 0) { if (pMD->RegulationMode & REGSTATE_SYNCHRONE) @@ -772,7 +782,11 @@ void dOutputRegulateMotor(UBYTE MotorNr) UBYTE SyncMotorTwo; MOTORDATA * pMD = &(MotorData[MotorNr]); - if (pMD->RegulationMode & REGSTATE_REGULATED) + if (pMD->RegulationMode & REGSTATE_POSITION) + { + dOutputAbsolutePositionRegulation(MotorNr); + } + else if (pMD->RegulationMode & REGSTATE_REGULATED) { dOutputCalculateMotorPosition(MotorNr); } @@ -790,6 +804,57 @@ void dOutputRegulateMotor(UBYTE MotorNr) } } +/* Absolute position regulation. */ +void dOutputAbsolutePositionRegulation(UBYTE MotorNr) +{ + /* Inputs: + * - CurrentCaptureCount: current motor position. + * - MotorTachoCountToRun: wanted position. + * + * - RegPParameter, RegIParameter, RegDParameter: PID parameters. + * + * Outputs: + * - MotorActualSpeed: power to be applied to motor. + * - MotorOverloaded: set if MotorActualSpeed reached maximum. + * + * - OldPositionError: remember last error, used for D part. + * - AccError: Accumulated error, used for I part. + */ + SLONG PositionError; + SLONG PValue, DValue, IValue, TotalRegValue; + + MOTORDATA *pMD = &MotorData[MotorNr]; + + PositionError = pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount; + PositionError = dOutputBound (PositionError, 32000); + + PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV); + PValue = dOutputBound (PValue, REG_MAX_VALUE); + + DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV); + pMD->OldPositionError = PositionError; + + pMD->AccError = (pMD->AccError * 3 + PositionError) / 4; + pMD->AccError = dOutputBound (pMD->AccError, 800); + + IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV); + IValue = dOutputBound (IValue, REG_MAX_VALUE); + + TotalRegValue = (PValue + IValue + DValue) / 2; + if (TotalRegValue > MAXIMUM_SPEED_FW) + { + TotalRegValue = MAXIMUM_SPEED_FW; + pMD->MotorOverloaded = 1; + } + else if (TotalRegValue < MAXIMUM_SPEED_RW) + { + TotalRegValue = MAXIMUM_SPEED_RW; + pMD->MotorOverloaded = 1; + } + + pMD->MotorActualSpeed = TotalRegValue; +} + /* Regulation function used when Position regulation is enabled */ /* The regulation form only control one motor at a time */ void dOutputCalculateMotorPosition(UBYTE MotorNr) diff --git a/AT91SAM7S256/Source/d_output.h b/AT91SAM7S256/Source/d_output.h index 1b0576a..1778414 100644 --- a/AT91SAM7S256/Source/d_output.h +++ b/AT91SAM7S256/Source/d_output.h @@ -43,6 +43,7 @@ #define REGSTATE_IDLE 0x00 #define REGSTATE_REGULATED 0x01 #define REGSTATE_SYNCHRONE 0x02 +#define REGSTATE_POSITION 0x04 //Constant reffering to RunState parameter #define MOTOR_RUN_STATE_IDLE 0x00 @@ -79,6 +80,7 @@ void dOutputCalculateRampUpParameter(UBYTE MotorNr, ULONG NewTachoLimit); void dOutputRampDownFunction(UBYTE MotorNr); void dOutputRampUpFunction(UBYTE MotorNr); void dOutputTachoLimitControl(UBYTE MotorNr); +void dOutputAbsolutePositionRegulation(UBYTE MotorNr); void dOutputCalculateMotorPosition(UBYTE MotorNr); void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo); void dOutputMotorReachedTachoLimit(UBYTE MotorNr); -- cgit v1.2.3