aboutsummaryrefslogtreecommitdiff
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.c51
1 files changed, 27 insertions, 24 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 3e9e1cd..8e5c904 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -28,7 +28,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
@@ -149,7 +148,7 @@ void dOutputInit(void)
pMD->RegDParameter = DEFAULT_D_GAIN_FACTOR;
pMD->MotorMaxSpeed = DEFAULT_MAX_SPEED;
pMD->MotorMaxAcceleration = DEFAULT_MAX_ACCELERATION;
- pMD->RegulationMode = 0;
+ pMD->RegulationMode = 0;
pMD->MotorOverloaded = 0;
pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE;
pMD->RampDownToLimit = 0;
@@ -205,6 +204,7 @@ void dOutputCtrl(void)
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
pMD->MotorRunState = MOTOR_RUN_STATE_RUNNING;
+
}
if (pMD->RegulationTimeCount > RegulationTime)
{
@@ -233,17 +233,17 @@ void dOutputGetMotorParameters(UBYTE *CurrentMotorSpeed, SLONG *TachoCount, SLON
{
MOTORDATA * pMD = &(MotorData[Tmp]);
CurrentMotorSpeed[Tmp] = pMD->MotorActualSpeed;
- TachoCount[Tmp] = pMD->CurrentCaptureCount;
- BlockTachoCount[Tmp] = pMD->MotorBlockTachoCount;
- RotationCount[Tmp] = pMD->RotationCaptureCount;
- RunState[Tmp] = pMD->MotorRunState;
- MotorOverloaded[Tmp] = pMD->MotorOverloaded;
+ TachoCount[Tmp] = pMD->CurrentCaptureCount;
+ BlockTachoCount[Tmp] = pMD->MotorBlockTachoCount;
+ RotationCount[Tmp] = pMD->RotationCaptureCount;
+ RunState[Tmp] = pMD->MotorRunState;
+ MotorOverloaded[Tmp] = pMD->MotorOverloaded;
}
}
-void dOutputSetMode(UBYTE Motor, UBYTE Mode) //Set motor mode (break, Float)
+void dOutputSetMode(UBYTE MotorNr, UBYTE Mode) //Set motor mode (break, Float)
{
- INSERTMode(Motor, Mode);
+ INSERTMode(MotorNr, Mode);
}
/* Update the regulation state for the motor */
@@ -395,7 +395,7 @@ void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel, UBYTE Opti
void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE NewTurnParameter)
{
MOTORDATA * pMD = &(MotorData[MotorNr]);
- if ((pMD->MotorSetSpeed != Speed) || (pMD->MotorRunState != NewMotorRunState) ||
+ if ((pMD->MotorSetSpeed != Speed) || (pMD->MotorRunState != NewMotorRunState) ||
(NewMotorRunState == MOTOR_RUN_STATE_IDLE) || (pMD->TurnParameter != NewTurnParameter))
{
if (pMD->MotorTargetSpeed == 0)
@@ -827,7 +827,7 @@ void dOutputMotorIdleControl(UBYTE MotorNr)
INSERTMode(MotorNr, COAST_MOTOR_MODE);
MOTORDATA * pMD = &(MotorData[MotorNr]);
-
+
if (pMD->MotorActualSpeed != 0)
{
if (pMD->MotorActualSpeed > 0)
@@ -1086,7 +1086,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
if (pOne->TurnParameter != 0)
{
- if ((pOne->MotorBlockTachoCount != 0) || (pTwo->MotorBlockTachoCount))
+ if ((pOne->MotorBlockTachoCount != 0) || (pTwo->MotorBlockTachoCount != 0))
{
if (pOne->MotorTargetSpeed >= 0)
{
@@ -1139,7 +1139,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
CorrectionValue = (PValue + IValue + DValue) / 4;
- MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue;
+ MotorSpeed = pOne->MotorTargetSpeed - CorrectionValue;
MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
if (pOne->TurnParameter != 0)
@@ -1155,7 +1155,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
}
pOne->MotorActualSpeed = (SBYTE)MotorSpeed;
- MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue;
+ MotorSpeed = pTwo->MotorTargetSpeed + CorrectionValue;
MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
if (pOne->TurnParameter != 0)
@@ -1190,18 +1190,18 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
break;
}
}
- pOne->MotorSetSpeed = 0;
+ pOne->MotorSetSpeed = 0;
pOne->MotorTargetSpeed = 0;
pOne->MotorActualSpeed = 0;
- pOne->MotorRunState = dOutputRunStateAtLimit(pOne);
- pOne->RegulationMode = dOutputRegModeAtLimit(pOne);
+ pOne->MotorRunState = dOutputRunStateAtLimit(pOne);
+ pOne->RegulationMode = dOutputRegModeAtLimit(pOne);
if (MotorTwo != 0xFF) {
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
- pTwo->MotorSetSpeed = 0;
+ pTwo->MotorSetSpeed = 0;
pTwo->MotorTargetSpeed = 0;
pTwo->MotorActualSpeed = 0;
- pTwo->MotorRunState = dOutputRunStateAtLimit(pTwo);
- pTwo->RegulationMode = dOutputRegModeAtLimit(pTwo);
+ pTwo->MotorRunState = dOutputRunStateAtLimit(pTwo);
+ pTwo->RegulationMode = dOutputRegModeAtLimit(pTwo);
}
}
else
@@ -1237,6 +1237,7 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
+
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pOne = &(MotorData[MotorOne]);
@@ -1403,20 +1404,21 @@ void dOutputResetSyncMotors(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
+
MOTORDATA * pMD = &(MotorData[MotorNr]);
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
- pMD->CurrentCaptureCount = 0;
- pMD->MotorTachoCountToRun = 0;
+ pMD->CurrentCaptureCount = 0;
+ pMD->MotorTachoCountToRun = 0;
pMD->MotorTachoCountTarget = 0;
- pTwo->CurrentCaptureCount = 0;
+ pTwo->CurrentCaptureCount = 0;
pTwo->MotorTachoCountToRun = 0;
pTwo->MotorTachoCountTarget = 0;
}
else
{
- pMD->CurrentCaptureCount = 0;
+ pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
pMD->MotorTachoCountTarget = 0;
}
@@ -1440,6 +1442,7 @@ void dOutputRampDownSynch(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
+
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pOne = &(MotorData[MotorOne]);