aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-10-17 18:33:40 +0200
committerNicolas Schodet2011-01-29 21:30:27 +0100
commitf3318727e0dc26e08157e9dd4e22ccbe29edc851 (patch)
tree3230809f6bb65a111a6f1e1e4bc58a124f0686f3
parent0f57e7c23cf5e9542992b12edbb279e1b3ac7083 (diff)
add fractional speed support for faster regulations
Original speed ranges from 0 to 100 per 100 ms. To keep the same range while lowering the regulation delay, a scaling must be done. This change scales the speed value and add support for fractional speed in order to keep the same granularity.
-rw-r--r--AT91SAM7S256/Source/d_output.c27
1 files changed, 25 insertions, 2 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 35e426b..29bc440 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -21,6 +21,7 @@
#define MAXIMUM_SPEED_RW -100
#define INPUT_SCALE_FACTOR 100
+#define SPEED_TIME 100
#define MAX_COUNT_TO_RUN 10000000
@@ -56,6 +57,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
@@ -98,6 +100,7 @@ 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;
@@ -152,6 +155,7 @@ 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;
@@ -209,6 +213,7 @@ void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode)
{
pMD->AccError = 0;
pMD->OldPositionError = 0;
+ pMD->PositionFracError = 0;
}
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
@@ -333,6 +338,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);
@@ -870,10 +876,27 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
SLONG IValue;
SLONG DValue;
SLONG TotalRegValue;
- SLONG NewSpeedCount = 0;
+ SLONG NewSpeed;
+ SLONG NewSpeedCount;
MOTORDATA * pMD = &(MotorData[MotorNr]);
- NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);
+ /* Apply fractional speed in case RegulationTime is different from
+ * SPEED_TIME. In this case, fractional part is accumulated until it reach
+ * one (with "one" being SPEED_TIME). This is use the same principle as the
+ * Bresenham algorithm. */
+ NewSpeed = pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR;
+ NewSpeedCount = NewSpeed * RegulationTime / SPEED_TIME;
+ pMD->PositionFracError += NewSpeed * RegulationTime % SPEED_TIME;
+ if (pMD->PositionFracError > SPEED_TIME)
+ {
+ pMD->PositionFracError -= SPEED_TIME;
+ NewSpeedCount++;
+ }
+ else if (pMD->PositionFracError < -SPEED_TIME)
+ {
+ pMD->PositionFracError += SPEED_TIME;
+ NewSpeedCount--;
+ }
PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;