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/c_output.c | 2 ++ AT91SAM7S256/Source/c_output.iom | 1 + AT91SAM7S256/Source/d_output.c | 23 +++++++++++++++++++---- AT91SAM7S256/Source/d_output.h | 4 ++++ 4 files changed, 26 insertions(+), 4 deletions(-) (limited to 'AT91SAM7S256') diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index c39f725..a016361 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -58,6 +58,7 @@ void cOutputInit(void* pHeader) pOut->Options = 0x00; } IOMapOutput.RegulationTime = REGULATION_TIME; + IOMapOutput.RegulationOptions = 0; VarsOutput.TimeCnt = 0; dOutputInit(); } @@ -137,6 +138,7 @@ void cOutputCtrl(void) } } dOutputSetRegulationTime(IOMapOutput.RegulationTime); + dOutputSetRegulationOptions(IOMapOutput.RegulationOptions); dOutputCtrl(); cOutputUpdateIomap(); } diff --git a/AT91SAM7S256/Source/c_output.iom b/AT91SAM7S256/Source/c_output.iom index e8a7579..09b935e 100644 --- a/AT91SAM7S256/Source/c_output.iom +++ b/AT91SAM7S256/Source/c_output.iom @@ -83,6 +83,7 @@ typedef struct { OUTPUT Outputs[NO_OF_OUTPUTS]; UBYTE RegulationTime; /* RW - Interval between regulation computations */ + UBYTE RegulationOptions; /* RW - Options for regulation, see REGOPTION_* */ }IOMAPOUTPUT; 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; diff --git a/AT91SAM7S256/Source/d_output.h b/AT91SAM7S256/Source/d_output.h index 1c24db0..41b1cfc 100644 --- a/AT91SAM7S256/Source/d_output.h +++ b/AT91SAM7S256/Source/d_output.h @@ -54,6 +54,9 @@ #define MOTOR_RUN_STATE_RAMPDOWN 0x40 #define MOTOR_RUN_STATE_HOLD 0x60 +// Constants related to Regulation Options +#define REGOPTION_NO_SATURATION 0x01 // Do not limit intermediary regulation results + enum { @@ -77,6 +80,7 @@ void dOutputResetBlockTachoLimit(UBYTE MotorNr); void dOutputResetRotationCaptureCount(UBYTE MotorNr); void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter); void dOutputSetRegulationTime(UBYTE NewRegulationTime); +void dOutputSetRegulationOptions(UBYTE NewRegulationOptions); void dOutputRegulateMotor(UBYTE MotorNr); void dOutputCalculateRampUpParameter(UBYTE MotorNr, ULONG NewTachoLimit); -- cgit v1.2.3