From fc1e658f15bbb8d4497d189e79b43fab4b964839 Mon Sep 17 00:00:00 2001 From: John Hansen Date: Sun, 13 Mar 2011 23:36:50 +0000 Subject: Added position regulation mode (Nicolas Schodet) git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc/branches/version_131@37 c9361245-7fe8-9947-84e8-057757c4e366 --- AT91SAM7S256/Source/c_cmd.c | 8 + AT91SAM7S256/Source/c_cmd.h | 4 +- AT91SAM7S256/Source/c_output.c | 19 +- AT91SAM7S256/Source/c_output.iom | 14 +- AT91SAM7S256/Source/d_output.c | 419 ++++++++++++++++++++++----------------- AT91SAM7S256/Source/d_output.h | 16 +- 6 files changed, 284 insertions(+), 196 deletions(-) diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index 231ebec..7a81c71 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -320,6 +320,8 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT TC_SLONG, //IO_OUT_ROTATION_COUNT TC_UBYTE, //IO_OUT_OPTIONS + TC_SBYTE, //IO_OUT_MAX_SPEED + TC_SBYTE, //IO_OUT_MAX_ACCELERATION //IO_OUT1 TC_UBYTE, //IO_OUT_FLAGS @@ -338,6 +340,8 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT TC_SLONG, //IO_OUT_ROTATION_COUNT TC_UBYTE, //IO_OUT_OPTIONS + TC_SBYTE, //IO_OUT_MAX_SPEED + TC_SBYTE, //IO_OUT_MAX_ACCELERATION //IO_OUT2 TC_UBYTE, //IO_OUT_FLAGS @@ -356,6 +360,8 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT TC_SLONG, //IO_OUT_ROTATION_COUNT TC_UBYTE, //IO_OUT_OPTIONS + TC_SBYTE, //IO_OUT_MAX_SPEED + TC_SBYTE, //IO_OUT_MAX_ACCELERATION }; @@ -1402,6 +1408,8 @@ void cCmdInit(void* pHeader) IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->BlockTachoCount); IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->RotationCount); IO_PTRS_OUT[IO_OUT_OPTIONS + i * IO_OUT_FPP] = (void*)&(pOut->Options); + IO_PTRS_OUT[IO_OUT_MAX_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->MaxSpeed); + IO_PTRS_OUT[IO_OUT_MAX_ACCELERATION + i * IO_OUT_FPP] = (void*)&(pOut->MaxAcceleration); } //Initialize IO_PTRS_IN diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h index a555e8b..bb4891f 100644 --- a/AT91SAM7S256/Source/c_cmd.h +++ b/AT91SAM7S256/Source/c_cmd.h @@ -207,9 +207,11 @@ enum IO_OUT_BLOCK_TACH_COUNT, IO_OUT_ROTATION_COUNT, IO_OUT_OPTIONS, + IO_OUT_MAX_SPEED, + IO_OUT_MAX_ACCELERATION, }; -#define IO_OUT_FPP 16 +#define IO_OUT_FPP 18 #define IO_OUT_FIELD_COUNT (IO_OUT_FPP * NO_OF_OUTPUTS) // diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index 212cc89..9e7c2e0 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -43,7 +43,6 @@ void cOutputInit(void* pHeader) { UBYTE Tmp; - IOMapOutput.PwnFreq = REGULATION_TIME; for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) { OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); @@ -57,7 +56,11 @@ void cOutputInit(void* pHeader) pOut->RegIParameter = DEFAULT_I_GAIN_FACTOR; pOut->RegDParameter = DEFAULT_D_GAIN_FACTOR; pOut->Options = 0x00; + pOut->MaxSpeed = DEFAULT_MAX_SPEED; + pOut->MaxAcceleration = DEFAULT_MAX_ACCELERATION; } + IOMapOutput.RegulationTime = REGULATION_TIME; + IOMapOutput.RegulationOptions = 0; VarsOutput.TimeCnt = 0; dOutputInit(); } @@ -66,7 +69,6 @@ void cOutputCtrl(void) { UBYTE Tmp; - dOutputUpdateRegulationTime(IOMapOutput.PwnFreq); for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) { OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); @@ -95,11 +97,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, pOut->Options); - } if (pOut->Flags & UPDATE_MODE) { pOut->Flags &= ~UPDATE_MODE; @@ -130,13 +127,21 @@ void cOutputCtrl(void) dOutputDisableRegulation(Tmp); } } + if (pOut->Flags & UPDATE_TACHO_LIMIT) + { + pOut->Flags &= ~UPDATE_TACHO_LIMIT; + dOutputSetTachoLimit(Tmp, pOut->TachoLimit, pOut->Options); + } if (pOut->Flags & UPDATE_PID_VALUES) { pOut->Flags &= ~UPDATE_PID_VALUES; dOutputSetPIDParameters(Tmp, pOut->RegPParameter, pOut->RegIParameter, pOut->RegDParameter); + dOutputSetMax(Tmp, pOut->MaxSpeed, pOut->MaxAcceleration); } } } + dOutputSetRegulationTime(IOMapOutput.RegulationTime); + dOutputSetRegulationOptions(IOMapOutput.RegulationOptions); dOutputCtrl(); cOutputUpdateIomap(); } diff --git a/AT91SAM7S256/Source/c_output.iom b/AT91SAM7S256/Source/c_output.iom index f6191bf..989a0c4 100644 --- a/AT91SAM7S256/Source/c_output.iom +++ b/AT91SAM7S256/Source/c_output.iom @@ -52,9 +52,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 @@ -76,15 +77,16 @@ typedef struct UBYTE Overloaded; /* R - True if the motor has been overloaded within speed control regulation */ SBYTE SyncTurnParameter; /* RW - Holds the turning parameter need within MoveBlock */ UBYTE Options; - UBYTE SpareTwo; - UBYTE SpareThree; + SBYTE MaxSpeed; /* RW - Maximum speed for absolute regulation, or 0 for no limit */ + SBYTE MaxAcceleration; /* RW - Maximum acceleration for absolute regulation, or 0 for no limit */ }OUTPUT; typedef struct { OUTPUT Outputs[NO_OF_OUTPUTS]; - UBYTE PwnFreq; // use for frequency of checking regulation mode + 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 73719cc..3e9e1cd 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -17,26 +17,38 @@ #include "d_output.h" #include "d_output.r" +#include + #define MAXIMUM_SPEED_FW 100 #define MAXIMUM_SPEED_RW -100 #define INPUT_SCALE_FACTOR 100 +#define SPEED_TIME 100 #define MAX_COUNT_TO_RUN 10000000 #define REG_MAX_VALUE 100 #define REG_MIN_VALUE -100 +#define RAMP_TIME_INTERVAL 25 // Measured in 1 mS => 25 mS interval + #define RAMPDOWN_STATE_RAMPDOWN 0 #define RAMPDOWN_STATE_CONTINIUE 1 #define COAST_MOTOR_MODE 0 +void dOutputRampDownSynch(UBYTE MotorNr); +SLONG dOutputBound(SLONG In, SLONG Limit); +SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError); +SLONG dOutputFractionalChange(SLONG Value, SWORD *FracError); +void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff); + +#define ABS(a) (((a) < 0) ? -(a) : (a)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + #define OPTION_HOLDATLIMIT 0x10 #define OPTION_RAMPDOWNTOLIMIT 0x20 -void dOutputRampDownSynch(UBYTE MotorNr); - typedef struct { SBYTE MotorSetSpeed; // Motor setpoint in speed @@ -57,6 +69,7 @@ typedef struct SWORD MotorRampUpIncrement; // Tell the number of count between each speed adjustment during Ramp-up SWORD AccError; // Accumulated Error, used within the integrator of the PID regulation SWORD OldPositionError; // Used within position regulation + SWORD PositionFracError; // Fractionnal position error of last position update SLONG DeltaCaptureCount; // Counts within last regulation time-periode SLONG CurrentCaptureCount; // Total counts since motor counts has been reset SLONG MotorTachoCountToRun; // Holds number of counts to run. 0 = Run forever @@ -64,6 +77,10 @@ typedef struct SLONG MotorRampTachoCountOld; // Used to hold old position during Ramp-Up SLONG MotorRampTachoCountStart; // Used to hold position when Ramp-up started SLONG RotationCaptureCount; // Counter for additional rotation counter + SLONG MotorTachoCountTarget; // For absolute regulation, position on which regulation is done + SWORD SpeedFracError; // Fractionnal speed error of last speed update + SBYTE MotorMaxSpeed; // For absolute regulation, maximum motor speed + SBYTE MotorMaxAcceleration; // For absolute regulation, maximum motor acceleration UBYTE RunStateAtLimit; // what run state to switch to when tacho limit is reached UBYTE RampDownToLimit; UBYTE Spare2; @@ -80,8 +97,8 @@ typedef struct static MOTORDATA MotorData[3]; static SYNCMOTORDATA SyncData; - -static UBYTE RegTime; +static UBYTE RegulationTime; +static UBYTE RegulationOptions; UBYTE dOutputRunStateAtLimit(MOTORDATA * pMD) { @@ -111,7 +128,7 @@ void dOutputInit(void) ENABLECaptureMotorB; ENABLECaptureMotorC; - RegTime = REGULATION_TIME; + RegulationTime = REGULATION_TIME; for (Temp = 0; Temp < 3; Temp++) { @@ -125,10 +142,13 @@ void dOutputInit(void) pMD->MotorTachoCountToRun = 0; pMD->MotorRunForever = 1; pMD->AccError = 0; + pMD->PositionFracError = 0; pMD->RegulationTimeCount = 0; pMD->RegPParameter = DEFAULT_P_GAIN_FACTOR; pMD->RegIParameter = DEFAULT_I_GAIN_FACTOR; pMD->RegDParameter = DEFAULT_D_GAIN_FACTOR; + pMD->MotorMaxSpeed = DEFAULT_MAX_SPEED; + pMD->MotorMaxAcceleration = DEFAULT_MAX_ACCELERATION; pMD->RegulationMode = 0; pMD->MotorOverloaded = 0; pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE; @@ -181,11 +201,12 @@ void dOutputCtrl(void) pMD->MotorSetSpeed = 0; pMD->MotorActualSpeed = 0; pMD->MotorTargetSpeed = 0; + pMD->PositionFracError = 0; pMD->RegulationTimeCount = 0; pMD->DeltaCaptureCount = 0; pMD->MotorRunState = MOTOR_RUN_STATE_RUNNING; } - if (pMD->RegulationTimeCount > RegTime) + if (pMD->RegulationTimeCount > RegulationTime) { pMD->RegulationTimeCount = 0; dOutputRegulateMotor(MotorNr); @@ -237,6 +258,7 @@ void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode) { pMD->AccError = 0; pMD->OldPositionError = 0; + pMD->PositionFracError = 0; } if (pMD->RegulationMode & REGSTATE_SYNCHRONE) @@ -265,6 +287,7 @@ void dOutputResetTachoLimit(UBYTE MotorNr) MOTORDATA * pMD = &(MotorData[MotorNr]); pMD->CurrentCaptureCount = 0; pMD->MotorTachoCountToRun = 0; + pMD->MotorTachoCountTarget = 0; if (pMD->RegulationMode & REGSTATE_SYNCHRONE) { @@ -300,13 +323,38 @@ void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewReg pMD->RegDParameter = NewRegDParameter; } +/* Set maximum speed and acceleration */ +void dOutputSetMax(UBYTE MotorNr, SBYTE NewMaxSpeed, SBYTE NewMaxAcceleration) +{ + MOTORDATA * pMD = &(MotorData[MotorNr]); + pMD->MotorMaxSpeed = NewMaxSpeed; + pMD->MotorMaxAcceleration = NewMaxAcceleration; +} + +/* Set new regulation time */ +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 */ void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel, UBYTE Options) { 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; pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE; @@ -354,6 +402,7 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE { pMD->AccError = 0; pMD->OldPositionError = 0; + pMD->PositionFracError = 0; pMD->RegulationTimeCount = 0; pMD->DeltaCaptureCount = 0; TACHOCountReset(MotorNr); @@ -696,6 +745,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) @@ -811,6 +865,17 @@ void dOutputMotorIdleControl(UBYTE MotorNr) } } +/* Check if Value is between [-Limit:Limit], and change it if it is not the case. */ +SLONG dOutputBound(SLONG Value, SLONG Limit) +{ + if (Value > Limit) + return Limit; + else if (Value < -Limit) + return -Limit; + else + return Value; +} + /* Function called to evaluate which regulation princip that need to run and which MotorNr to use (I.E.: Which motors are synched together)*/ void dOutputRegulateMotor(UBYTE MotorNr) { @@ -818,7 +883,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); } @@ -836,92 +905,167 @@ void dOutputRegulateMotor(UBYTE MotorNr) } } -/* Regulation function used when Position regulation is enabled */ -/* The regulation form only control one motor at a time */ -void dOutputCalculateMotorPosition(UBYTE MotorNr) +/* Compute PID regulation result for a given error. */ +SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError) { - SWORD PositionError; - SWORD PValue; - SWORD IValue; - SWORD DValue; - SWORD TotalRegValue; - SWORD NewSpeedCount = 0; + SLONG PValue, DValue, IValue, TotalRegValue; - MOTORDATA * pMD = &(MotorData[MotorNr]); - NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR); + MOTORDATA *pMD = &MotorData[MotorNr]; + + PositionError = dOutputBound (PositionError, 32000); + + PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV); + + DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV); + pMD->OldPositionError = PositionError; - PositionError = (SWORD)(pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount; + pMD->AccError = (pMD->AccError * 3 + PositionError) / 4; + pMD->AccError = dOutputBound (pMD->AccError, 800); - //Overflow control on PositionError - if (pMD->RegPParameter != 0) + IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV); + + if (!(RegulationOptions & REGOPTION_NO_SATURATION)) { - if (PositionError > (SWORD)(32000 / pMD->RegPParameter)) - { - PositionError = (SWORD)(32000 / pMD->RegPParameter); - } - if (PositionError < (SWORD)(-(32000 / pMD->RegPParameter))) - { - PositionError = (SWORD)(-(32000 / pMD->RegPParameter)); - } + PValue = dOutputBound (PValue, REG_MAX_VALUE); + IValue = dOutputBound (IValue, REG_MAX_VALUE); } - else + + TotalRegValue = (PValue + IValue + DValue) / 2; + + if (TotalRegValue > MAXIMUM_SPEED_FW) { - if (PositionError > (SWORD)32000) - { - PositionError = (SWORD)32000; - } - if (PositionError < (SWORD)-32000) - { - PositionError = (SWORD)-32000; - } + TotalRegValue = MAXIMUM_SPEED_FW; + pMD->MotorOverloaded = 1; + } + else if (TotalRegValue < MAXIMUM_SPEED_RW) + { + TotalRegValue = MAXIMUM_SPEED_RW; + pMD->MotorOverloaded = 1; } - PValue = PositionError * (SWORD)(pMD->RegPParameter/REG_CONST_DIV); - if (PValue > (SWORD)REG_MAX_VALUE) + return TotalRegValue; +} + +/* Compute integer change for this regulation step, according to value and + * previous fractional error. + * Used for values which are expressed as "per SPEED_TIME" to translate them + * in "per RegulationTime".*/ +SLONG dOutputFractionalChange(SLONG Value, SWORD *FracError) +{ + SLONG IntegerChange; + + /* Apply fractional change in case RegulationTime is different from + * SPEED_TIME. In this case, fractional part is accumulated until it reach + * one half (with "one" being SPEED_TIME). This is use the same principle + * as the Bresenham algorithm. */ + IntegerChange = Value * RegulationTime / SPEED_TIME; + *FracError += Value * RegulationTime % SPEED_TIME; + if (*FracError > SPEED_TIME / 2) { - PValue = REG_MAX_VALUE; + *FracError -= SPEED_TIME; + IntegerChange++; } - if (PValue <= (SWORD)REG_MIN_VALUE) + else if (*FracError < -SPEED_TIME / 2) { - PValue = REG_MIN_VALUE; + *FracError += SPEED_TIME; + IntegerChange--; } - DValue = (PositionError - pMD->OldPositionError) * (SWORD)(pMD->RegDParameter/REG_CONST_DIV); - pMD->OldPositionError = PositionError; - - pMD->AccError = (pMD->AccError * 3) + PositionError; - pMD->AccError = pMD->AccError / 4; + return IntegerChange; +} - if (pMD->AccError > (SWORD)800) +/* Filter speed according to motor maximum speed and acceleration. */ +void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff) +{ + /* Inputs: + * - PositionDiff: difference between current position and position to reach. + * - MotorMaxAcceleration: maximum speed change per regulation period (or 0 for unlimited). + * - MotorMaxSpeed: maximum motor speed (can not be zero, or do not call this function). + * Output: + * - MotorTargetSpeed: speed to regulate on motor. + */ + MOTORDATA *pMD = &MotorData[MotorNr]; + SLONG IdealSpeed; + SLONG PositionDiffAbs = ABS (PositionDiff); + /* Should be able to brake on time. */ + if (pMD->MotorMaxAcceleration + && PositionDiffAbs < MAXIMUM_SPEED_FW * MAXIMUM_SPEED_FW / 2) { - pMD->AccError = 800; + IdealSpeed = sqrtf (2 * PositionDiffAbs * pMD->MotorMaxAcceleration); + IdealSpeed = dOutputBound (IdealSpeed, pMD->MotorMaxSpeed); } - if (pMD->AccError <= (SWORD)-800) + else { - pMD->AccError = -800; + /* Do not go past consign. */ + IdealSpeed = MIN (PositionDiffAbs, pMD->MotorMaxSpeed); } - IValue = pMD->AccError * (SWORD)(pMD->RegIParameter/REG_CONST_DIV); - - if (IValue > (SWORD)REG_MAX_VALUE) + /* Apply sign. */ + if (PositionDiff < 0) { - IValue = REG_MAX_VALUE; + IdealSpeed = -IdealSpeed; } - if (IValue <= (SWORD)REG_MIN_VALUE) + /* Check max acceleration. */ + SLONG SpeedDiff = IdealSpeed - pMD->MotorTargetSpeed; + if (pMD->MotorMaxAcceleration) { - IValue = REG_MIN_VALUE; + SLONG MaxSpeedChange = dOutputFractionalChange (pMD->MotorMaxAcceleration, &pMD->SpeedFracError); + SpeedDiff = dOutputBound (SpeedDiff, MaxSpeedChange); } - TotalRegValue = (SWORD)((PValue + IValue + DValue)/2); + pMD->MotorTargetSpeed += SpeedDiff; +} - if (TotalRegValue > MAXIMUM_SPEED_FW) +/* Absolute position regulation. */ +void dOutputAbsolutePositionRegulation(UBYTE MotorNr) +{ + /* Inputs: + * - CurrentCaptureCount: current motor position. + * - MotorTachoCountToRun: wanted position, filtered with speed and acceleration. + * + * Outputs: + * - MotorActualSpeed: power to be applied to motor. + * - MotorOverloaded: set if MotorActualSpeed reached maximum. + */ + SLONG PositionChange; + SLONG PositionError; + SLONG TotalRegValue; + + MOTORDATA *pMD = &MotorData[MotorNr]; + + /* Position update. */ + if (pMD->MotorMaxSpeed) { - TotalRegValue = MAXIMUM_SPEED_FW; - pMD->MotorOverloaded = 1; + dOutputSpeedFilter (MotorNr, pMD->MotorTachoCountToRun - pMD->MotorTachoCountTarget); + PositionChange = dOutputFractionalChange (pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR, &pMD->PositionFracError); + pMD->MotorTachoCountTarget += PositionChange; } - if (TotalRegValue < MAXIMUM_SPEED_RW) + else { - TotalRegValue = MAXIMUM_SPEED_RW; - pMD->MotorOverloaded = 1; + pMD->MotorTachoCountTarget = pMD->MotorTachoCountToRun; } + + /* Regulation. */ + PositionError = pMD->MotorTachoCountTarget - pMD->CurrentCaptureCount; + TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError); + + 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) +{ + SLONG PositionError; + SLONG TotalRegValue; + SLONG PositionChange; + + MOTORDATA * pMD = &(MotorData[MotorNr]); + + PositionChange = dOutputFractionalChange (pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR, &pMD->PositionFracError); + + PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + PositionChange; + + TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError); + pMD->MotorActualSpeed = (SBYTE)TotalRegValue; } @@ -930,11 +1074,11 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) { SLONG TempTurnParameter; - SWORD PValue; - SWORD IValue; - SWORD DValue; - SWORD CorrectionValue; - SWORD MotorSpeed; + SLONG PValue; + SLONG IValue; + SLONG DValue; + SLONG CorrectionValue; + SLONG MotorSpeed; MOTORDATA * pOne = &(MotorData[MotorOne]); MOTORDATA * pTwo = &(MotorData[MotorTwo]); @@ -981,136 +1125,48 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) //SyncTurnParameter should ophold difference between the two motors. SyncData.SyncTachoDif += SyncData.SyncTurnParameter; + SyncData.SyncTachoDif = dOutputBound (SyncData.SyncTachoDif, 500); - if (SyncData.SyncTachoDif > 500) - { - SyncData.SyncTachoDif = 500; - } - if (SyncData.SyncTachoDif < -500) - { - SyncData.SyncTachoDif = -500; - } + PValue = SyncData.SyncTachoDif * (pOne->RegPParameter/REG_CONST_DIV); - /* - if ((SWORD)SyncData.SyncTachoDif > 500) - { - SyncData.SyncTachoDif = 500; - } - if ((SWORD)SyncData.SyncTachoDif < -500) - { - SyncData.SyncTachoDif = -500; - } - */ - - PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV); - - DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(pOne->RegDParameter/REG_CONST_DIV); + DValue = (SyncData.SyncTachoDif - SyncData.SyncOldError) * (pOne->RegDParameter/REG_CONST_DIV); SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif; SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif; + SyncData.SyncAccError = dOutputBound (SyncData.SyncAccError, 900); - if (SyncData.SyncAccError > (SWORD)900) - { - SyncData.SyncAccError = 900; - } - if (SyncData.SyncAccError < (SWORD)-900) - { - SyncData.SyncAccError = -900; - } - IValue = SyncData.SyncAccError * (SWORD)(pOne->RegIParameter/REG_CONST_DIV); + IValue = SyncData.SyncAccError * (pOne->RegIParameter/REG_CONST_DIV); - CorrectionValue = (SWORD)((PValue + IValue + DValue)/4); + CorrectionValue = (PValue + IValue + DValue) / 4; MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue; - - if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW) - { - MotorSpeed = MAXIMUM_SPEED_FW; - } - else - { - if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW) - { - MotorSpeed = MAXIMUM_SPEED_RW; - } - } + MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW); if (pOne->TurnParameter != 0) { if (pOne->MotorTargetSpeed > 0) { - if (MotorSpeed > (SWORD)pOne->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pOne->MotorTargetSpeed; - } - else - { - if (MotorSpeed < (SWORD)-pOne->MotorTargetSpeed) - { - MotorSpeed = -pOne->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, pOne->MotorTargetSpeed); } else { - if (MotorSpeed < (SWORD)pOne->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pOne->MotorTargetSpeed; - } - else - { - if (MotorSpeed > (SWORD)-pOne->MotorTargetSpeed) - { - MotorSpeed = -pOne->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, -pOne->MotorTargetSpeed); } } pOne->MotorActualSpeed = (SBYTE)MotorSpeed; MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue; - - if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW) - { - MotorSpeed = MAXIMUM_SPEED_FW; - } - else - { - if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW) - { - MotorSpeed = MAXIMUM_SPEED_RW; - } - } + MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW); if (pOne->TurnParameter != 0) { if (pTwo->MotorTargetSpeed > 0) { - if (MotorSpeed > (SWORD)pTwo->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pTwo->MotorTargetSpeed; - } - else - { - if (MotorSpeed < (SWORD)-pTwo->MotorTargetSpeed) - { - MotorSpeed = -pTwo->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, pTwo->MotorTargetSpeed); } else { - if (MotorSpeed < (SWORD)pTwo->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pTwo->MotorTargetSpeed; - } - else - { - if (MotorSpeed > (SWORD)-pTwo->MotorTargetSpeed) - { - MotorSpeed = -pTwo->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, -pTwo->MotorTargetSpeed); } } pTwo->MotorActualSpeed = (SBYTE)MotorSpeed; @@ -1125,7 +1181,8 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) UBYTE MotorOne, MotorTwo; MotorOne = MotorNr; MotorTwo = 0xFF; - for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) { + UBYTE i; + for(i = MOTOR_A; i <= MOTOR_C; i++) { if (i == MotorOne) continue; if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) { @@ -1167,7 +1224,10 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr) MotorOne = MotorNr; MotorTwo = 0xFF; - for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) { + // Synchronisation is done two times, as this function is called for each + // motor. This is the same behaviour as previous code. + UBYTE i; + for(i = MOTOR_A; i <= MOTOR_C; i++) { if (i == MotorOne) continue; if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) { @@ -1332,7 +1392,8 @@ void dOutputResetSyncMotors(UBYTE MotorNr) MotorOne = MotorNr; MotorTwo = 0xFF; - for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) { + UBYTE i; + for(i = MOTOR_A; i <= MOTOR_C; i++) { if (i == MotorOne) continue; if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) { @@ -1348,13 +1409,16 @@ void dOutputResetSyncMotors(UBYTE MotorNr) MOTORDATA * pTwo = &(MotorData[MotorTwo]); pMD->CurrentCaptureCount = 0; pMD->MotorTachoCountToRun = 0; + pMD->MotorTachoCountTarget = 0; pTwo->CurrentCaptureCount = 0; pTwo->MotorTachoCountToRun = 0; + pTwo->MotorTachoCountTarget = 0; } else { pMD->CurrentCaptureCount = 0; pMD->MotorTachoCountToRun = 0; + pMD->MotorTachoCountTarget = 0; } } @@ -1365,7 +1429,8 @@ void dOutputRampDownSynch(UBYTE MotorNr) MotorOne = MotorNr; MotorTwo = 0xFF; - for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) { + UBYTE i; + for(i = MOTOR_A; i <= MOTOR_C; i++) { if (i == MotorOne) continue; if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) { @@ -1419,7 +1484,3 @@ void dOutputRampDownSynch(UBYTE MotorNr) } } -void dOutputUpdateRegulationTime(UBYTE rt) -{ - RegTime = rt; -} diff --git a/AT91SAM7S256/Source/d_output.h b/AT91SAM7S256/Source/d_output.h index ce78246..20955a6 100644 --- a/AT91SAM7S256/Source/d_output.h +++ b/AT91SAM7S256/Source/d_output.h @@ -39,10 +39,17 @@ #endif +#define DEFAULT_MAX_SPEED 80 +#define DEFAULT_MAX_ACCELERATION 20 + +#define REGULATION_TIME 100 // Measured in 1 mS => 100 mS regulation interval +//#define REGULATION_TIME 10 // Measured in 1 mS, regulation interval + //Constant reffering to RegMode parameter #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 @@ -51,9 +58,8 @@ #define MOTOR_RUN_STATE_RAMPDOWN 0x40 #define MOTOR_RUN_STATE_HOLD 0x60 - -#define RAMP_TIME_INTERVAL 25 // Measured in 1 mS => 25 mS interval -#define REGULATION_TIME 100 // Measured in 1 mS => 100 mS regulation interval +// Constants related to Regulation Options +#define REGOPTION_NO_SATURATION 0x01 // Do not limit intermediary regulation results enum { @@ -76,12 +82,16 @@ void dOutputResetTachoLimit(UBYTE MotorNr); void dOutputResetBlockTachoLimit(UBYTE MotorNr); void dOutputResetRotationCaptureCount(UBYTE MotorNr); void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter); +void dOutputSetMax(UBYTE MotorNr, SBYTE NewMaxSpeed, SBYTE NewMaxAcceleration); +void dOutputSetRegulationTime(UBYTE NewRegulationTime); +void dOutputSetRegulationOptions(UBYTE NewRegulationOptions); void dOutputRegulateMotor(UBYTE MotorNr); 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