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.c380
1 files changed, 315 insertions, 65 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 3608cb7..8e5c904 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -17,17 +17,19 @@
#include "d_output.h"
#include "d_output.r"
+#include <math.h>
+
#define MAXIMUM_SPEED_FW 100
#define MAXIMUM_SPEED_RW -100
#define INPUT_SCALE_FACTOR 100
+#define SPEED_TIME 100
#define MAX_COUNT_TO_RUN 10000000
#define REG_MAX_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
#define RAMPDOWN_STATE_RAMPDOWN 0
#define RAMPDOWN_STATE_CONTINIUE 1
@@ -36,6 +38,15 @@
void dOutputRampDownSynch(UBYTE MotorNr);
SLONG dOutputBound(SLONG In, SLONG Limit);
+SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError);
+SLONG dOutputFractionalChange(SLONG Value, SWORD *FracError);
+void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff);
+
+#define ABS(a) (((a) < 0) ? -(a) : (a))
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+
+#define OPTION_HOLDATLIMIT 0x10
+#define OPTION_RAMPDOWNTOLIMIT 0x20
typedef struct
{
@@ -57,6 +68,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 PositionFracError; // Fractionnal position 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
@@ -64,6 +76,14 @@ typedef struct
SLONG MotorRampTachoCountOld; // Used to hold old position during Ramp-Up
SLONG MotorRampTachoCountStart; // Used to hold position when Ramp-up started
SLONG RotationCaptureCount; // Counter for additional rotation counter
+ SLONG MotorTachoCountTarget; // For absolute regulation, position on which regulation is done
+ SWORD SpeedFracError; // Fractionnal speed error of last speed update
+ SBYTE MotorMaxSpeed; // For absolute regulation, maximum motor speed
+ SBYTE MotorMaxAcceleration; // For absolute regulation, maximum motor acceleration
+ UBYTE RunStateAtLimit; // what run state to switch to when tacho limit is reached
+ UBYTE RampDownToLimit;
+ UBYTE Spare2;
+ UBYTE Spare3;
}MOTORDATA;
typedef struct
@@ -76,6 +96,27 @@ typedef struct
static MOTORDATA MotorData[3];
static SYNCMOTORDATA SyncData;
+static UBYTE RegulationTime;
+static UBYTE RegulationOptions;
+
+UBYTE dOutputRunStateAtLimit(MOTORDATA * pMD)
+{
+// return MOTOR_RUN_STATE_IDLE;
+ return pMD->RunStateAtLimit;
+}
+
+UBYTE dOutputRampDownToLimit(MOTORDATA * pMD)
+{
+// return 0;
+ return pMD->RampDownToLimit;
+}
+
+UBYTE dOutputRegModeAtLimit(MOTORDATA * pMD)
+{
+ if (dOutputRunStateAtLimit(pMD)==MOTOR_RUN_STATE_HOLD)
+ return REGSTATE_REGULATED;
+ return REGSTATE_IDLE;
+}
void dOutputInit(void)
{
@@ -85,6 +126,8 @@ void dOutputInit(void)
ENABLECaptureMotorA;
ENABLECaptureMotorB;
ENABLECaptureMotorC;
+
+ RegulationTime = REGULATION_TIME;
for (Temp = 0; Temp < 3; Temp++)
{
@@ -98,18 +141,23 @@ void dOutputInit(void)
pMD->MotorTachoCountToRun = 0;
pMD->MotorRunForever = 1;
pMD->AccError = 0;
+ pMD->PositionFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->RegPParameter = DEFAULT_P_GAIN_FACTOR;
pMD->RegIParameter = DEFAULT_I_GAIN_FACTOR;
pMD->RegDParameter = DEFAULT_D_GAIN_FACTOR;
+ pMD->MotorMaxSpeed = DEFAULT_MAX_SPEED;
+ pMD->MotorMaxAcceleration = DEFAULT_MAX_ACCELERATION;
pMD->RegulationMode = 0;
pMD->MotorOverloaded = 0;
+ pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE;
+ pMD->RampDownToLimit = 0;
INSERTMode(Temp, COAST_MOTOR_MODE);
INSERTSpeed(Temp, pMD->MotorSetSpeed);
}
}
-/* This function is called every 1 mS and will go through all the motors and there dependencies */
+/* This function is called every 1 mS and will go through all the motors and their dependencies */
/* Actual motor speed is only passed (updated) to the AVR controller form this function */
/* DeltacaptureCount used to count number of Tachocount within last 100 mS. Used with position control regulation */
/* CurrentCaptureCount used to tell total current position. Used to tell when movement has been obtained */
@@ -152,12 +200,13 @@ void dOutputCtrl(void)
pMD->MotorSetSpeed = 0;
pMD->MotorActualSpeed = 0;
pMD->MotorTargetSpeed = 0;
+ pMD->PositionFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
pMD->MotorRunState = MOTOR_RUN_STATE_RUNNING;
}
- if (pMD->RegulationTimeCount > REGULATION_TIME)
+ if (pMD->RegulationTimeCount > RegulationTime)
{
pMD->RegulationTimeCount = 0;
dOutputRegulateMotor(MotorNr);
@@ -209,6 +258,7 @@ void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode)
{
pMD->AccError = 0;
pMD->OldPositionError = 0;
+ pMD->PositionFracError = 0;
}
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
@@ -237,6 +287,7 @@ void dOutputResetTachoLimit(UBYTE MotorNr)
MOTORDATA * pMD = &(MotorData[MotorNr]);
pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
+ pMD->MotorTachoCountTarget = 0;
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
{
@@ -272,19 +323,48 @@ void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewReg
pMD->RegDParameter = NewRegDParameter;
}
+/* Set maximum speed and acceleration */
+void dOutputSetMax(UBYTE MotorNr, SBYTE NewMaxSpeed, SBYTE NewMaxAcceleration)
+{
+ MOTORDATA * pMD = &(MotorData[MotorNr]);
+ pMD->MotorMaxSpeed = NewMaxSpeed;
+ pMD->MotorMaxAcceleration = NewMaxAcceleration;
+}
+
+/* Set new regulation time */
+void dOutputSetRegulationTime(UBYTE NewRegulationTime)
+{
+ RegulationTime = NewRegulationTime;
+}
+
+/* Set new regulation options */
+void dOutputSetRegulationOptions(UBYTE NewRegulationOptions)
+{
+ RegulationOptions = NewRegulationOptions;
+}
+
/* Called to set TachoCountToRun which is used for position control for the model */
/* Must be called before motor start */
/* TachoCountToRun is calculated as a signed value */
-void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel)
+void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel, UBYTE Options)
{
MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (BlockTachoCntToTravel == 0)
+ if (pMD->RegulationMode & REGSTATE_POSITION)
+ {
+ pMD->MotorRunForever = 0;
+ pMD->MotorTachoCountToRun = BlockTachoCntToTravel;
+ }
+ else if (BlockTachoCntToTravel == 0)
{
pMD->MotorRunForever = 1;
+ pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE;
+ pMD->RampDownToLimit = 0;
}
else
{
pMD->MotorRunForever = 0;
+ pMD->RunStateAtLimit = (Options & OPTION_HOLDATLIMIT) ? MOTOR_RUN_STATE_HOLD : MOTOR_RUN_STATE_IDLE;
+ pMD->RampDownToLimit = Options & OPTION_RAMPDOWNTOLIMIT;
if (pMD->MotorSetSpeed == 0)
{
@@ -322,6 +402,7 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE
{
pMD->AccError = 0;
pMD->OldPositionError = 0;
+ pMD->PositionFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
TACHOCountReset(MotorNr);
@@ -515,7 +596,7 @@ void dOutputRampUpFunction(UBYTE MotorNr)
if ((pMD->CurrentCaptureCount - pMD->MotorRampTachoCountStart) >= (pMD->MotorTachoCountToRun - pMD->MotorRampTachoCountStart))
{
pMD->MotorTargetSpeed = pMD->MotorSetSpeed;
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pMD->MotorRunState = dOutputRunStateAtLimit(pMD);
}
}
else
@@ -523,7 +604,7 @@ void dOutputRampUpFunction(UBYTE MotorNr)
if ((pMD->CurrentCaptureCount + pMD->MotorRampTachoCountStart) <= (pMD->MotorTachoCountToRun + pMD->MotorRampTachoCountStart))
{
pMD->MotorTargetSpeed = pMD->MotorSetSpeed;
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pMD->MotorRunState = dOutputRunStateAtLimit(pMD);
}
}
if (pMD->MotorSetSpeed > 0)
@@ -632,7 +713,7 @@ void dOutputRampDownFunction(UBYTE MotorNr)
if ((pMD->RegulationMode & REGSTATE_SYNCHRONE) && (pMD->TurnParameter != 0))
{
dOutputSyncTachoLimitControl(MotorNr);
- if (pMD->MotorRunState == MOTOR_RUN_STATE_IDLE)
+ if (pMD->MotorRunState == dOutputRunStateAtLimit(pMD))
{
dOutputMotorReachedTachoLimit(MotorNr);
}
@@ -664,6 +745,11 @@ void dOutputRampDownFunction(UBYTE MotorNr)
void dOutputTachoLimitControl(UBYTE MotorNr)
{
MOTORDATA * pMD = &(MotorData[MotorNr]);
+ if (pMD->RegulationMode & REGSTATE_POSITION)
+ {
+ /* No limit when doing absolute position regulation. */
+ return;
+ }
if (pMD->MotorRunForever == 0)
{
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
@@ -672,22 +758,47 @@ void dOutputTachoLimitControl(UBYTE MotorNr)
}
else
{
- if (pMD->MotorSetSpeed > 0)
+ if (dOutputRampDownToLimit(pMD) == 0)
{
- if ((pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun))
+ if (pMD->MotorSetSpeed > 0)
{
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pMD->RegulationMode = REGSTATE_IDLE;
+ if ((pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun))
+ {
+ pMD->MotorRunState = dOutputRunStateAtLimit(pMD);
+ pMD->RegulationMode = dOutputRegModeAtLimit(pMD);
+ }
+ }
+ else
+ {
+ if (pMD->MotorSetSpeed < 0)
+ {
+ if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun)
+ {
+ pMD->MotorRunState = dOutputRunStateAtLimit(pMD);
+ pMD->RegulationMode = dOutputRegModeAtLimit(pMD);
+ }
+ }
}
}
else
{
- if (pMD->MotorSetSpeed < 0)
+ if (pMD->MotorSetSpeed > 0)
+ {
+ if ((pMD->CurrentCaptureCount >= (SLONG)((float)pMD->MotorTachoCountToRun * (float)0.80)))
+ {
+ pMD->MotorRunState = MOTOR_RUN_STATE_RAMPDOWN;
+ pMD->MotorSetSpeed = 0;
+ }
+ }
+ else
{
- if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun)
+ if (pMD->MotorSetSpeed < 0)
{
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pMD->RegulationMode = REGSTATE_IDLE;
+ if (pMD->CurrentCaptureCount <= (SLONG)((float)pMD->MotorTachoCountToRun * (float)0.80))
+ {
+ pMD->MotorRunState = MOTOR_RUN_STATE_RAMPDOWN;
+ pMD->MotorSetSpeed = 0;
+ }
}
}
}
@@ -772,7 +883,11 @@ void dOutputRegulateMotor(UBYTE MotorNr)
UBYTE SyncMotorTwo;
MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (pMD->RegulationMode & REGSTATE_REGULATED)
+ if (pMD->RegulationMode & REGSTATE_POSITION)
+ {
+ dOutputAbsolutePositionRegulation(MotorNr);
+ }
+ else if (pMD->RegulationMode & REGSTATE_REGULATED)
{
dOutputCalculateMotorPosition(MotorNr);
}
@@ -790,37 +905,30 @@ void dOutputRegulateMotor(UBYTE MotorNr)
}
}
-/* Regulation function used when Position regulation is enabled */
-/* The regulation form only control one motor at a time */
-void dOutputCalculateMotorPosition(UBYTE MotorNr)
+/* Compute PID regulation result for a given error. */
+SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError)
{
- 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);
+ SLONG PValue, DValue, IValue, TotalRegValue;
- PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;
+ MOTORDATA *pMD = &MotorData[MotorNr];
- //Overflow control on PositionError
PositionError = dOutputBound (PositionError, 32000);
PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV);
- PValue = dOutputBound (PValue, REG_MAX_VALUE);
DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV);
- pMD->OldPositionError = (SWORD)PositionError;
+ pMD->OldPositionError = PositionError;
- pMD->AccError = (pMD->AccError * 3) + (SWORD)PositionError;
- pMD->AccError = pMD->AccError / 4;
+ pMD->AccError = (pMD->AccError * 3 + PositionError) / 4;
pMD->AccError = dOutputBound (pMD->AccError, 800);
IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV);
- IValue = dOutputBound (IValue, REG_MAX_VALUE);
+
+ if (!(RegulationOptions & REGOPTION_NO_SATURATION))
+ {
+ PValue = dOutputBound (PValue, REG_MAX_VALUE);
+ IValue = dOutputBound (IValue, REG_MAX_VALUE);
+ }
TotalRegValue = (PValue + IValue + DValue) / 2;
@@ -829,11 +937,135 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
TotalRegValue = MAXIMUM_SPEED_FW;
pMD->MotorOverloaded = 1;
}
- if (TotalRegValue < MAXIMUM_SPEED_RW)
+ else if (TotalRegValue < MAXIMUM_SPEED_RW)
{
TotalRegValue = MAXIMUM_SPEED_RW;
pMD->MotorOverloaded = 1;
}
+
+ return TotalRegValue;
+}
+
+/* Compute integer change for this regulation step, according to value and
+ * previous fractional error.
+ * Used for values which are expressed as "per SPEED_TIME" to translate them
+ * in "per RegulationTime".*/
+SLONG dOutputFractionalChange(SLONG Value, SWORD *FracError)
+{
+ SLONG IntegerChange;
+
+ /* Apply fractional change in case RegulationTime is different from
+ * SPEED_TIME. In this case, fractional part is accumulated until it reach
+ * one half (with "one" being SPEED_TIME). This is use the same principle
+ * as the Bresenham algorithm. */
+ IntegerChange = Value * RegulationTime / SPEED_TIME;
+ *FracError += Value * RegulationTime % SPEED_TIME;
+ if (*FracError > SPEED_TIME / 2)
+ {
+ *FracError -= SPEED_TIME;
+ IntegerChange++;
+ }
+ else if (*FracError < -SPEED_TIME / 2)
+ {
+ *FracError += SPEED_TIME;
+ IntegerChange--;
+ }
+
+ return IntegerChange;
+}
+
+/* Filter speed according to motor maximum speed and acceleration. */
+void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff)
+{
+ /* Inputs:
+ * - PositionDiff: difference between current position and position to reach.
+ * - MotorMaxAcceleration: maximum speed change per regulation period (or 0 for unlimited).
+ * - MotorMaxSpeed: maximum motor speed (can not be zero, or do not call this function).
+ * Output:
+ * - MotorTargetSpeed: speed to regulate on motor.
+ */
+ MOTORDATA *pMD = &MotorData[MotorNr];
+ SLONG IdealSpeed;
+ SLONG PositionDiffAbs = ABS (PositionDiff);
+ /* Should be able to brake on time. */
+ if (pMD->MotorMaxAcceleration
+ && PositionDiffAbs < MAXIMUM_SPEED_FW * MAXIMUM_SPEED_FW / 2)
+ {
+ IdealSpeed = sqrtf (2 * PositionDiffAbs * pMD->MotorMaxAcceleration);
+ IdealSpeed = dOutputBound (IdealSpeed, pMD->MotorMaxSpeed);
+ }
+ else
+ {
+ /* Do not go past consign. */
+ IdealSpeed = MIN (PositionDiffAbs, pMD->MotorMaxSpeed);
+ }
+ /* Apply sign. */
+ if (PositionDiff < 0)
+ {
+ IdealSpeed = -IdealSpeed;
+ }
+ /* Check max acceleration. */
+ SLONG SpeedDiff = IdealSpeed - pMD->MotorTargetSpeed;
+ if (pMD->MotorMaxAcceleration)
+ {
+ SLONG MaxSpeedChange = dOutputFractionalChange (pMD->MotorMaxAcceleration, &pMD->SpeedFracError);
+ SpeedDiff = dOutputBound (SpeedDiff, MaxSpeedChange);
+ }
+ pMD->MotorTargetSpeed += SpeedDiff;
+}
+
+/* Absolute position regulation. */
+void dOutputAbsolutePositionRegulation(UBYTE MotorNr)
+{
+ /* Inputs:
+ * - CurrentCaptureCount: current motor position.
+ * - MotorTachoCountToRun: wanted position, filtered with speed and acceleration.
+ *
+ * Outputs:
+ * - MotorActualSpeed: power to be applied to motor.
+ * - MotorOverloaded: set if MotorActualSpeed reached maximum.
+ */
+ SLONG PositionChange;
+ SLONG PositionError;
+ SLONG TotalRegValue;
+
+ MOTORDATA *pMD = &MotorData[MotorNr];
+
+ /* Position update. */
+ if (pMD->MotorMaxSpeed)
+ {
+ dOutputSpeedFilter (MotorNr, pMD->MotorTachoCountToRun - pMD->MotorTachoCountTarget);
+ PositionChange = dOutputFractionalChange (pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR, &pMD->PositionFracError);
+ pMD->MotorTachoCountTarget += PositionChange;
+ }
+ else
+ {
+ pMD->MotorTachoCountTarget = pMD->MotorTachoCountToRun;
+ }
+
+ /* Regulation. */
+ PositionError = pMD->MotorTachoCountTarget - pMD->CurrentCaptureCount;
+ TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError);
+
+ pMD->MotorActualSpeed = TotalRegValue;
+}
+
+/* Regulation function used when Position regulation is enabled */
+/* The regulation form only control one motor at a time */
+void dOutputCalculateMotorPosition(UBYTE MotorNr)
+{
+ SLONG PositionError;
+ SLONG TotalRegValue;
+ SLONG PositionChange;
+
+ MOTORDATA * pMD = &(MotorData[MotorNr]);
+
+ PositionChange = dOutputFractionalChange (pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR, &pMD->PositionFracError);
+
+ PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + PositionChange;
+
+ TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError);
+
pMD->MotorActualSpeed = (SBYTE)TotalRegValue;
}
@@ -961,15 +1193,15 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
pOne->MotorSetSpeed = 0;
pOne->MotorTargetSpeed = 0;
pOne->MotorActualSpeed = 0;
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pOne->RegulationMode = REGSTATE_IDLE;
+ pOne->MotorRunState = dOutputRunStateAtLimit(pOne);
+ pOne->RegulationMode = dOutputRegModeAtLimit(pOne);
if (MotorTwo != 0xFF) {
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
pTwo->MotorSetSpeed = 0;
pTwo->MotorTargetSpeed = 0;
pTwo->MotorActualSpeed = 0;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->RegulationMode = REGSTATE_IDLE;
+ pTwo->MotorRunState = dOutputRunStateAtLimit(pTwo);
+ pTwo->RegulationMode = dOutputRegModeAtLimit(pTwo);
}
}
else
@@ -979,8 +1211,8 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
pOne->MotorTargetSpeed = 0;
pOne->MotorActualSpeed = 0;
}
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pOne->RegulationMode = REGSTATE_IDLE;
+ pOne->MotorRunState = dOutputRunStateAtLimit(pOne);
+ pOne->RegulationMode = dOutputRegModeAtLimit(pOne);
}
}
@@ -1010,30 +1242,44 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr)
{
MOTORDATA * pOne = &(MotorData[MotorOne]);
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
+ SLONG l1 = pOne->MotorTachoCountToRun;
+ SLONG l2 = pTwo->MotorTachoCountToRun;
+ UBYTE NewRunState1 = dOutputRunStateAtLimit(pOne);
+ UBYTE NewRunState2 = dOutputRunStateAtLimit(pTwo);
+ if (dOutputRampDownToLimit(pOne) == OPTION_RAMPDOWNTOLIMIT)
+ {
+ NewRunState1 = MOTOR_RUN_STATE_RAMPDOWN;
+ l1 = (SLONG)((float)l1 * 0.80f);
+ }
+ if (dOutputRampDownToLimit(pTwo) == OPTION_RAMPDOWNTOLIMIT)
+ {
+ NewRunState2 = MOTOR_RUN_STATE_RAMPDOWN;
+ l2 = (SLONG)((float)l2 * 0.80f);
+ }
if (pOne->TurnParameter != 0)
{
if (pOne->TurnParameter > 0)
{
if (pTwo->MotorTargetSpeed >= 0)
{
- if ((SLONG)(pTwo->CurrentCaptureCount >= pTwo->MotorTachoCountToRun))
+ if ((SLONG)(pTwo->CurrentCaptureCount >= l2))
{
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pOne->MotorRunState = NewRunState1;
+ pTwo->MotorRunState = NewRunState2;
pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount;
- pOne->MotorTachoCountToRun = pTwo->MotorTachoCountToRun;
+ pOne->MotorTachoCountToRun = l2;
}
}
else
{
- if ((SLONG)(pOne->CurrentCaptureCount <= pOne->MotorTachoCountToRun))
+ if ((SLONG)(pOne->CurrentCaptureCount <= l1))
{
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pOne->MotorRunState = NewRunState1;
+ pTwo->MotorRunState = NewRunState2;
pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount;
- pTwo->MotorTachoCountToRun = pOne->MotorTachoCountToRun;
+ pTwo->MotorTachoCountToRun = l1;
}
}
}
@@ -1041,46 +1287,47 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr)
{
if (pOne->MotorTargetSpeed >= 0)
{
- if ((SLONG)(pOne->CurrentCaptureCount >= pOne->MotorTachoCountToRun))
+ if ((SLONG)(pOne->CurrentCaptureCount >= l1))
{
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pOne->MotorRunState = NewRunState1;
+ pTwo->MotorRunState = NewRunState2;
pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount;
- pTwo->MotorTachoCountToRun = pOne->MotorTachoCountToRun;
+ pTwo->MotorTachoCountToRun = l1;
}
}
else
{
- if ((SLONG)(pTwo->CurrentCaptureCount <= pTwo->MotorTachoCountToRun))
+ if ((SLONG)(pTwo->CurrentCaptureCount <= l2))
{
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pOne->MotorRunState = NewRunState1;
+ pTwo->MotorRunState = NewRunState2;
pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount;
- pOne->MotorTachoCountToRun = pTwo->MotorTachoCountToRun;
+ pOne->MotorTachoCountToRun = l2;
}
}
}
}
else
{
+ // no turning
if (pOne->MotorSetSpeed > 0)
{
- if ((pOne->CurrentCaptureCount >= pOne->MotorTachoCountToRun) || (pTwo->CurrentCaptureCount >= pTwo->MotorTachoCountToRun))
+ if ((pOne->CurrentCaptureCount >= l1) || (pTwo->CurrentCaptureCount >= l2))
{
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pOne->MotorRunState = NewRunState1;
+ pTwo->MotorRunState = NewRunState2;
}
}
else
{
if (pOne->MotorSetSpeed < 0)
{
- if ((pOne->CurrentCaptureCount <= pOne->MotorTachoCountToRun) || (pTwo->CurrentCaptureCount <= pTwo->MotorTachoCountToRun))
+ if ((pOne->CurrentCaptureCount <= l1) || (pTwo->CurrentCaptureCount <= l2))
{
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
+ pOne->MotorRunState = NewRunState1;
+ pTwo->MotorRunState = NewRunState2;
}
}
}
@@ -1164,13 +1411,16 @@ void dOutputResetSyncMotors(UBYTE MotorNr)
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
+ pMD->MotorTachoCountTarget = 0;
pTwo->CurrentCaptureCount = 0;
pTwo->MotorTachoCountToRun = 0;
+ pTwo->MotorTachoCountTarget = 0;
}
else
{
pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
+ pMD->MotorTachoCountTarget = 0;
}
}