aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-10-17 18:33:40 +0200
committerNicolas Schodet2011-01-21 23:41:18 +0100
commit20c12f1de0abb5e7cb7152dbdcf95ffcaf79b779 (patch)
tree9efb852e3eae0977916bdd80b131ed736d4c8ee3
parente57ee876faf432f95ad5e8b857702037ac1afcfe (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.c24
1 files changed, 22 insertions, 2 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index ce5867f..a299ed9 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
@@ -63,6 +64,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 MotorSpeedCountFracError; // Speed 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
@@ -105,6 +107,7 @@ void dOutputInit(void)
pMD->MotorTachoCountToRun = 0;
pMD->MotorRunForever = 1;
pMD->AccError = 0;
+ pMD->MotorSpeedCountFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->RegPParameter = DEFAULT_P_GAIN_FACTOR;
pMD->RegIParameter = DEFAULT_I_GAIN_FACTOR;
@@ -159,6 +162,7 @@ void dOutputCtrl(void)
pMD->MotorSetSpeed = 0;
pMD->MotorActualSpeed = 0;
pMD->MotorTargetSpeed = 0;
+ pMD->MotorSpeedCountFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
pMD->MotorRunState = MOTOR_RUN_STATE_RUNNING;
@@ -340,6 +344,7 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE
{
pMD->AccError = 0;
pMD->OldPositionError = 0;
+ pMD->MotorSpeedCountFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
TACHOCountReset(MotorNr);
@@ -866,10 +871,24 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
SWORD IValue;
SWORD DValue;
SWORD TotalRegValue;
- SWORD NewSpeedCount = 0;
+ SWORD NewSpeed;
+ SWORD NewSpeedCount;
MOTORDATA * pMD = &(MotorData[MotorNr]);
- NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);
+ /* Apply fractionnal speed if RegulationTime is different from SPEED_TIME */
+ NewSpeed = pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR;
+ NewSpeedCount = NewSpeed * RegulationTime / SPEED_TIME;
+ pMD->MotorSpeedCountFracError += NewSpeed * RegulationTime % SPEED_TIME;
+ if (pMD->MotorSpeedCountFracError > SPEED_TIME)
+ {
+ pMD->MotorSpeedCountFracError -= SPEED_TIME;
+ NewSpeedCount++;
+ }
+ else if (pMD->MotorSpeedCountFracError < -SPEED_TIME)
+ {
+ pMD->MotorSpeedCountFracError += SPEED_TIME;
+ NewSpeedCount--;
+ }
PositionError = (SWORD)(pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;
@@ -1175,6 +1194,7 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
{
pOne->MotorTargetSpeed = 0;
pOne->MotorActualSpeed = 0;
+ pOne->MotorSpeedCountFracError = 0;
}
pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
pOne->RegulationMode = REGSTATE_IDLE;