aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-01-29 14:17:20 +0100
committerNicolas Schodet2011-01-30 00:59:42 +0100
commit041bed29f50fcb8bb601b6233f77787ef7031605 (patch)
tree129cfb46682cacf64017e50a44c3736ae2582887
parent26331f872603b3a9c745f9549f59d0548f048da4 (diff)
WIP: speed filter
-rw-r--r--AT91SAM7S256/Source/d_output.c69
1 files 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 <math.h>
+
#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;
}
}