aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-01-29 19:03:04 +0100
committerNicolas Schodet2011-01-29 21:30:28 +0100
commit26331f872603b3a9c745f9549f59d0548f048da4 (patch)
tree7d21fd13f82f6d866ae22d290a0bc44c35a1e460
parentf871606a785d9b7d99011e6cd05beca98f9f2376 (diff)
move fractional position error code in its own function
-rw-r--r--AT91SAM7S256/Source/d_output.c56
1 files changed, 36 insertions, 20 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 608e4b0..b6974ed 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -37,6 +37,7 @@
void dOutputRampDownSynch(UBYTE MotorNr);
SLONG dOutputBound(SLONG In, SLONG Limit);
SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError);
+SLONG dOutputPositionChange(UBYTE MotorNr, SLONG Speed);
typedef struct
{
@@ -865,6 +866,37 @@ SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError)
return TotalRegValue;
}
+/* Compute position change for this regulation step, according to speed and
+ * previous position fractional error. */
+SLONG dOutputPositionChange(UBYTE MotorNr, SLONG Speed)
+{
+ SLONG NewSpeed;
+ SLONG PositionChange;
+
+ MOTORDATA *pMD = &MotorData[MotorNr];
+
+ NewSpeed = Speed * 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. */
+ PositionChange = NewSpeed * RegulationTime / SPEED_TIME;
+ pMD->PositionFracError += NewSpeed * RegulationTime % SPEED_TIME;
+ if (pMD->PositionFracError > SPEED_TIME)
+ {
+ pMD->PositionFracError -= SPEED_TIME;
+ PositionChange++;
+ }
+ else if (pMD->PositionFracError < -SPEED_TIME)
+ {
+ pMD->PositionFracError += SPEED_TIME;
+ PositionChange--;
+ }
+
+ return PositionChange;
+}
+
/* Absolute position regulation. */
void dOutputAbsolutePositionRegulation(UBYTE MotorNr)
{
@@ -894,29 +926,13 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
{
SLONG PositionError;
SLONG TotalRegValue;
- SLONG NewSpeed;
- SLONG NewSpeedCount;
+ SLONG PositionChange;
MOTORDATA * pMD = &(MotorData[MotorNr]);
- /* 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;
+ PositionChange = dOutputPositionChange (MotorNr, pMD->MotorTargetSpeed);
+
+ PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + PositionChange;
TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError);