summaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source/d_output.c
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256/Source/d_output.c')
-rw-r--r--AT91SAM7S256/Source/d_output.c215
1 files changed, 48 insertions, 167 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 63613f9..3608cb7 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -25,7 +25,6 @@
#define MAX_COUNT_TO_RUN 10000000
#define REG_MAX_VALUE 100
-#define REG_MIN_VALUE -100
#define RAMP_TIME_INTERVAL 25 // Measured in 1 mS => 25 mS interval
#define REGULATION_TIME 100 // Measured in 1 mS => 100 mS regulation interval
@@ -36,6 +35,7 @@
#define COAST_MOTOR_MODE 0
void dOutputRampDownSynch(UBYTE MotorNr);
+SLONG dOutputBound(SLONG In, SLONG Limit);
typedef struct
{
@@ -754,6 +754,17 @@ void dOutputMotorIdleControl(UBYTE MotorNr)
}
}
+/* Check if Value is between [-Limit:Limit], and change it if it is not the case. */
+SLONG dOutputBound(SLONG Value, SLONG Limit)
+{
+ if (Value > Limit)
+ return Limit;
+ else if (Value < -Limit)
+ return -Limit;
+ else
+ return Value;
+}
+
/* Function called to evaluate which regulation princip that need to run and which MotorNr to use (I.E.: Which motors are synched together)*/
void dOutputRegulateMotor(UBYTE MotorNr)
{
@@ -783,77 +794,35 @@ void dOutputRegulateMotor(UBYTE MotorNr)
/* The regulation form only control one motor at a time */
void dOutputCalculateMotorPosition(UBYTE MotorNr)
{
- SWORD PositionError;
- SWORD PValue;
- SWORD IValue;
- SWORD DValue;
- SWORD TotalRegValue;
- SWORD NewSpeedCount = 0;
+ SLONG PositionError;
+ SLONG PValue;
+ SLONG IValue;
+ SLONG DValue;
+ SLONG TotalRegValue;
+ SLONG NewSpeedCount = 0;
MOTORDATA * pMD = &(MotorData[MotorNr]);
NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);
- PositionError = (SWORD)(pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;
+ PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;
//Overflow control on PositionError
- if (pMD->RegPParameter != 0)
- {
- if (PositionError > (SWORD)(32000 / pMD->RegPParameter))
- {
- PositionError = (SWORD)(32000 / pMD->RegPParameter);
- }
- if (PositionError < (SWORD)(-(32000 / pMD->RegPParameter)))
- {
- PositionError = (SWORD)(-(32000 / pMD->RegPParameter));
- }
- }
- else
- {
- if (PositionError > (SWORD)32000)
- {
- PositionError = (SWORD)32000;
- }
- if (PositionError < (SWORD)-32000)
- {
- PositionError = (SWORD)-32000;
- }
- }
+ PositionError = dOutputBound (PositionError, 32000);
- PValue = PositionError * (SWORD)(pMD->RegPParameter/REG_CONST_DIV);
- if (PValue > (SWORD)REG_MAX_VALUE)
- {
- PValue = REG_MAX_VALUE;
- }
- if (PValue <= (SWORD)REG_MIN_VALUE)
- {
- PValue = REG_MIN_VALUE;
- }
+ PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV);
+ PValue = dOutputBound (PValue, REG_MAX_VALUE);
- DValue = (PositionError - pMD->OldPositionError) * (SWORD)(pMD->RegDParameter/REG_CONST_DIV);
- pMD->OldPositionError = PositionError;
+ DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV);
+ pMD->OldPositionError = (SWORD)PositionError;
- pMD->AccError = (pMD->AccError * 3) + PositionError;
+ pMD->AccError = (pMD->AccError * 3) + (SWORD)PositionError;
pMD->AccError = pMD->AccError / 4;
+ pMD->AccError = dOutputBound (pMD->AccError, 800);
- if (pMD->AccError > (SWORD)800)
- {
- pMD->AccError = 800;
- }
- if (pMD->AccError <= (SWORD)-800)
- {
- pMD->AccError = -800;
- }
- IValue = pMD->AccError * (SWORD)(pMD->RegIParameter/REG_CONST_DIV);
+ IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV);
+ IValue = dOutputBound (IValue, REG_MAX_VALUE);
- if (IValue > (SWORD)REG_MAX_VALUE)
- {
- IValue = REG_MAX_VALUE;
- }
- if (IValue <= (SWORD)REG_MIN_VALUE)
- {
- IValue = REG_MIN_VALUE;
- }
- TotalRegValue = (SWORD)((PValue + IValue + DValue)/2);
+ TotalRegValue = (PValue + IValue + DValue) / 2;
if (TotalRegValue > MAXIMUM_SPEED_FW)
{
@@ -873,11 +842,11 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
{
SLONG TempTurnParameter;
- SWORD PValue;
- SWORD IValue;
- SWORD DValue;
- SWORD CorrectionValue;
- SWORD MotorSpeed;
+ SLONG PValue;
+ SLONG IValue;
+ SLONG DValue;
+ SLONG CorrectionValue;
+ SLONG MotorSpeed;
MOTORDATA * pOne = &(MotorData[MotorOne]);
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
@@ -924,136 +893,48 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
//SyncTurnParameter should ophold difference between the two motors.
SyncData.SyncTachoDif += SyncData.SyncTurnParameter;
+ SyncData.SyncTachoDif = dOutputBound (SyncData.SyncTachoDif, 500);
- if (SyncData.SyncTachoDif > 500)
- {
- SyncData.SyncTachoDif = 500;
- }
- if (SyncData.SyncTachoDif < -500)
- {
- SyncData.SyncTachoDif = -500;
- }
-
- /*
- if ((SWORD)SyncData.SyncTachoDif > 500)
- {
- SyncData.SyncTachoDif = 500;
- }
- if ((SWORD)SyncData.SyncTachoDif < -500)
- {
- SyncData.SyncTachoDif = -500;
- }
- */
-
- PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV);
+ PValue = SyncData.SyncTachoDif * (pOne->RegPParameter/REG_CONST_DIV);
- DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(pOne->RegDParameter/REG_CONST_DIV);
+ DValue = (SyncData.SyncTachoDif - SyncData.SyncOldError) * (pOne->RegDParameter/REG_CONST_DIV);
SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif;
SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif;
+ SyncData.SyncAccError = dOutputBound (SyncData.SyncAccError, 900);
- if (SyncData.SyncAccError > (SWORD)900)
- {
- SyncData.SyncAccError = 900;
- }
- if (SyncData.SyncAccError < (SWORD)-900)
- {
- SyncData.SyncAccError = -900;
- }
- IValue = SyncData.SyncAccError * (SWORD)(pOne->RegIParameter/REG_CONST_DIV);
+ IValue = SyncData.SyncAccError * (pOne->RegIParameter/REG_CONST_DIV);
- CorrectionValue = (SWORD)((PValue + IValue + DValue)/4);
+ CorrectionValue = (PValue + IValue + DValue) / 4;
- MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue;
-
- if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
- {
- MotorSpeed = MAXIMUM_SPEED_FW;
- }
- else
- {
- if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
- {
- MotorSpeed = MAXIMUM_SPEED_RW;
- }
- }
+ MotorSpeed = pOne->MotorTargetSpeed - CorrectionValue;
+ MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
if (pOne->TurnParameter != 0)
{
if (pOne->MotorTargetSpeed > 0)
{
- if (MotorSpeed > (SWORD)pOne->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pOne->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed < (SWORD)-pOne->MotorTargetSpeed)
- {
- MotorSpeed = -pOne->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, pOne->MotorTargetSpeed);
}
else
{
- if (MotorSpeed < (SWORD)pOne->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pOne->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed > (SWORD)-pOne->MotorTargetSpeed)
- {
- MotorSpeed = -pOne->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, -pOne->MotorTargetSpeed);
}
}
pOne->MotorActualSpeed = (SBYTE)MotorSpeed;
- MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue;
-
- if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
- {
- MotorSpeed = MAXIMUM_SPEED_FW;
- }
- else
- {
- if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
- {
- MotorSpeed = MAXIMUM_SPEED_RW;
- }
- }
+ MotorSpeed = pTwo->MotorTargetSpeed + CorrectionValue;
+ MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
if (pOne->TurnParameter != 0)
{
if (pTwo->MotorTargetSpeed > 0)
{
- if (MotorSpeed > (SWORD)pTwo->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pTwo->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed < (SWORD)-pTwo->MotorTargetSpeed)
- {
- MotorSpeed = -pTwo->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, pTwo->MotorTargetSpeed);
}
else
{
- if (MotorSpeed < (SWORD)pTwo->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pTwo->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed > (SWORD)-pTwo->MotorTargetSpeed)
- {
- MotorSpeed = -pTwo->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, -pTwo->MotorTargetSpeed);
}
}
pTwo->MotorActualSpeed = (SBYTE)MotorSpeed;