From 041bed29f50fcb8bb601b6233f77787ef7031605 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 29 Jan 2011 14:17:20 +0100 Subject: WIP: speed filter --- AT91SAM7S256/Source/d_output.c | 69 ++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 67 insertions(+), 2 deletions(-) diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index b6974ed..b244e8e 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -17,6 +17,8 @@ #include "d_output.h" #include "d_output.r" +#include + #define MAXIMUM_SPEED_FW 100 #define MAXIMUM_SPEED_RW -100 @@ -38,6 +40,10 @@ void dOutputRampDownSynch(UBYTE MotorNr); SLONG dOutputBound(SLONG In, SLONG Limit); SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError); SLONG dOutputPositionChange(UBYTE MotorNr, SLONG Speed); +void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff); + +#define ABS(a) (((a) < 0) ? -(a) : (a)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) typedef struct { @@ -67,6 +73,9 @@ 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 + SLONG MotorMaxSpeed; // For absolute regulation, maximum motor speed + SLONG MotorMaxAcceleration; // For absolute regulation, maximum motor acceleration }MOTORDATA; typedef struct @@ -245,6 +254,7 @@ void dOutputResetTachoLimit(UBYTE MotorNr) MOTORDATA * pMD = &(MotorData[MotorNr]); pMD->CurrentCaptureCount = 0; pMD->MotorTachoCountToRun = 0; + pMD->MotorTachoCountTarget = 0; if (pMD->RegulationMode & REGSTATE_SYNCHRONE) { @@ -897,24 +907,76 @@ SLONG dOutputPositionChange(UBYTE MotorNr, SLONG Speed) return PositionChange; } +/* 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) + { + IdealSpeed = sqrtf (2 * PositionDiffAbs * pMD->MotorMaxAcceleration); + IdealSpeed = dOutputBound (IdealSpeed, pMD->MotorMaxSpeed); + } + else + { + /* Do not go past consign. */ + IdealSpeed = MIN (PositionDiffAbs, pMD->MotorMaxSpeed); + } + /* Apply sign. */ + if (PositionDiff < 0) + { + IdealSpeed = -IdealSpeed; + } + /* Check max acceleration. */ + SLONG SpeedDiff = IdealSpeed - pMD->MotorTargetSpeed; + if (pMD->MotorMaxAcceleration) + { + SpeedDiff = dOutputBound (SpeedDiff, pMD->MotorMaxAcceleration); + } + pMD->MotorTargetSpeed += SpeedDiff; +} + /* Absolute position regulation. */ void dOutputAbsolutePositionRegulation(UBYTE MotorNr) { /* Inputs: * - CurrentCaptureCount: current motor position. - * - MotorTachoCountToRun: wanted 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]; - PositionError = pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount; + /* Position update. */ + if (pMD->MotorMaxSpeed) + { + dOutputSpeedFilter (MotorNr, pMD->MotorTachoCountToRun - pMD->MotorTachoCountTarget); + PositionChange = dOutputPositionChange (MotorNr, pMD->MotorTargetSpeed); + pMD->MotorTachoCountTarget += PositionChange; + } + else + { + pMD->MotorTachoCountTarget = pMD->MotorTachoCountToRun; + } + /* Regulation. */ + PositionError = pMD->MotorTachoCountTarget - pMD->CurrentCaptureCount; TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError); pMD->MotorActualSpeed = TotalRegValue; @@ -1266,13 +1328,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; } } -- cgit v1.2.3