From 4b496037c82a9950f21c0fc8f13a099d124d42e3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 17 Oct 2010 18:33:40 +0200 Subject: 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. --- AT91SAM7S256/Source/d_output.c | 24 ++++++++++++++++++++++-- 1 file 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; -- cgit v1.2.3