aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-10-17 18:33:40 +0200
committerNicolas Schodet2010-11-01 10:33:33 +0100
commit4b496037c82a9950f21c0fc8f13a099d124d42e3 (patch)
treea305d5f9cf149a45c1d27de6c9b173e24571bda7
parent678b07dfbfe4ad554fae5e228aadb0aeb3c7d04f (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 df8035a..29e1b96 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
@@ -104,6 +106,7 @@ void dOutputInit(void)
MotorData[Temp].MotorTachoCountToRun = 0;
MotorData[Temp].MotorRunForever = 1;
MotorData[Temp].AccError = 0;
+ MotorData[Temp].MotorSpeedCountFracError = 0;
MotorData[Temp].RegulationTimeCount = 0;
MotorData[Temp].RegPParameter = DEFAULT_P_GAIN_FACTOR;
MotorData[Temp].RegIParameter = DEFAULT_I_GAIN_FACTOR;
@@ -157,6 +160,7 @@ void dOutputCtrl(void)
MotorData[MotorNr].MotorSetSpeed = 0;
MotorData[MotorNr].MotorActualSpeed = 0;
MotorData[MotorNr].MotorTargetSpeed = 0;
+ MotorData[MotorNr].MotorSpeedCountFracError = 0;
MotorData[MotorNr].RegulationTimeCount = 0;
MotorData[MotorNr].DeltaCaptureCount = 0;
MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_RUNNING;
@@ -331,6 +335,7 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE
{
MotorData[MotorNr].AccError = 0;
MotorData[MotorNr].OldPositionError = 0;
+ MotorData[MotorNr].MotorSpeedCountFracError = 0;
MotorData[MotorNr].RegulationTimeCount = 0;
MotorData[MotorNr].DeltaCaptureCount = 0;
TACHOCountReset(MotorNr);
@@ -850,9 +855,23 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
SWORD IValue;
SWORD DValue;
SWORD TotalRegValue;
- SWORD NewSpeedCount = 0;
+ SWORD NewSpeed;
+ SWORD NewSpeedCount;
- NewSpeedCount = (SWORD)((MotorData[MotorNr].MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);
+ /* Apply fractionnal speed if RegulationTime is different from SPEED_TIME */
+ NewSpeed = MotorData[MotorNr].MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR;
+ NewSpeedCount = NewSpeed * RegulationTime / SPEED_TIME;
+ MotorData[MotorNr].MotorSpeedCountFracError += NewSpeed * RegulationTime % SPEED_TIME;
+ if (MotorData[MotorNr].MotorSpeedCountFracError > SPEED_TIME)
+ {
+ MotorData[MotorNr].MotorSpeedCountFracError -= SPEED_TIME;
+ NewSpeedCount++;
+ }
+ else if (MotorData[MotorNr].MotorSpeedCountFracError < -SPEED_TIME)
+ {
+ MotorData[MotorNr].MotorSpeedCountFracError += SPEED_TIME;
+ NewSpeedCount--;
+ }
PositionError = (SWORD)(MotorData[MotorNr].OldPositionError - MotorData[MotorNr].DeltaCaptureCount) + NewSpeedCount;
@@ -1265,6 +1284,7 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
MotorData[MotorNr].MotorSetSpeed = 0;
MotorData[MotorNr].MotorTargetSpeed = 0;
MotorData[MotorNr].MotorActualSpeed = 0;
+ MotorData[MotorNr].MotorSpeedCountFracError = 0;
}
MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;