From 9ac9938748901f8f6f6f73fa751349cdcc1dcd27 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 29 Jan 2011 00:54:42 +0100 Subject: add option to disable saturation in regulation intermediary values When computing PID, the output code limit the value of P and I participation. This is a problem as this introduces non-linearities and limits the efficiency of P and I terms. --- AT91SAM7S256/Source/d_output.c | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) (limited to 'AT91SAM7S256/Source/d_output.c') diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index 29bc440..192a242 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -78,6 +78,7 @@ typedef struct static MOTORDATA MotorData[3]; static SYNCMOTORDATA SyncData; static UBYTE RegulationTime; +static UBYTE RegulationOptions; void dOutputInit(void) { @@ -283,6 +284,12 @@ void dOutputSetRegulationTime(UBYTE NewRegulationTime) RegulationTime = NewRegulationTime; } +/* Set new regulation options */ +void dOutputSetRegulationOptions(UBYTE NewRegulationOptions) +{ + RegulationOptions = NewRegulationOptions; +} + /* Called to set TachoCountToRun which is used for position control for the model */ /* Must be called before motor start */ /* TachoCountToRun is calculated as a signed value */ @@ -841,7 +848,6 @@ void dOutputAbsolutePositionRegulation(UBYTE MotorNr) 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; @@ -850,7 +856,12 @@ void dOutputAbsolutePositionRegulation(UBYTE MotorNr) pMD->AccError = dOutputBound (pMD->AccError, 800); IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV); - IValue = dOutputBound (IValue, REG_MAX_VALUE); + + if (!(RegulationOptions & REGOPTION_NO_SATURATION)) + { + PValue = dOutputBound (PValue, REG_MAX_VALUE); + IValue = dOutputBound (IValue, REG_MAX_VALUE); + } TotalRegValue = (PValue + IValue + DValue) / 2; if (TotalRegValue > MAXIMUM_SPEED_FW) @@ -904,7 +915,6 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) 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 = (SWORD)PositionError; @@ -914,7 +924,12 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) pMD->AccError = dOutputBound (pMD->AccError, 800); IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV); - IValue = dOutputBound (IValue, REG_MAX_VALUE); + + if (!(RegulationOptions & REGOPTION_NO_SATURATION)) + { + PValue = dOutputBound (PValue, REG_MAX_VALUE); + IValue = dOutputBound (IValue, REG_MAX_VALUE); + } TotalRegValue = (PValue + IValue + DValue) / 2; -- cgit v1.2.3