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.c1616
1 files changed, 1616 insertions, 0 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
new file mode 100644
index 0000000..d953b84
--- /dev/null
+++ b/AT91SAM7S256/Source/d_output.c
@@ -0,0 +1,1616 @@
+//
+// Date init 14.12.2004
+//
+// Revision date $Date:: 3-02-09 14:46 $
+//
+// Filename $Workfile:: d_output.c $
+//
+// Version $Revision:: 2 $
+//
+// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/d_outp $
+//
+// Platform C
+//
+
+#include "stdconst.h"
+#include "m_sched.h"
+#include "d_output.h"
+#include "d_output.r"
+
+#define MAXIMUM_SPEED_FW 100
+#define MAXIMUM_SPEED_RW -100
+
+#define INPUT_SCALE_FACTOR 100
+
+#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
+
+#define RAMPDOWN_STATE_RAMPDOWN 0
+#define RAMPDOWN_STATE_CONTINIUE 1
+
+#define COAST_MOTOR_MODE 0
+
+void dOutputRampDownSynch(UBYTE MotorNr);
+
+typedef struct
+{
+ SBYTE MotorSetSpeed; // Motor setpoint in speed
+ SBYTE MotorTargetSpeed; // Speed order for the movement
+ SBYTE MotorActualSpeed; // Actual speed for motor (Calculated within the PID regulation)
+ SBYTE TurnParameter; // Tell the turning parameter used
+ UBYTE RegPParameter; // Current P parameter used within the regulation
+ UBYTE RegIParameter; // Current I parameter used within the regulation
+ UBYTE RegDParameter; // Current D parameter used within the regulation
+ UBYTE RegulationTimeCount; // Time counter used to evaluate when the regulation should run again (100 mS)
+ UBYTE MotorRunState; // Hold current motor state (Ramp-up, Running, Ramp-Down, Idle)
+ UBYTE RegulationMode; // Hold current regulation mode (Position control, Synchronization mode)
+ UBYTE MotorOverloaded; // Set if the motor speed in regulation is calculated to be above maximum
+ UBYTE MotorRunForever; // Tell that the motor is set to run forever
+ UWORD MotorRampDownCount; // Counter to tell if the ramp-down can reach it gaol and therefor need some additional help
+ SWORD MotorRampDownIncrement; // Tell the number of count between each speed adjustment during Ramp-Down
+ UWORD MotorRampUpCount; // Used to speedup Ramp-Up if position regulation is not enabled
+ 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
+ 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
+ SLONG MotorBlockTachoCount; // Hold CaptureCount for current movement
+ 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
+}MOTORDATA;
+
+typedef struct
+{
+ SLONG SyncTachoDif;
+ SLONG SyncTurnParameter;
+ SWORD SyncOldError;
+ SWORD SyncAccError;
+}SYNCMOTORDATA;
+
+static MOTORDATA MotorData[3];
+static SYNCMOTORDATA SyncData;
+
+void dOutputInit(void)
+{
+ UBYTE Temp;
+
+ OUTPUTInit;
+ ENABLECaptureMotorA;
+ ENABLECaptureMotorB;
+ ENABLECaptureMotorC;
+
+ for (Temp = 0; Temp < 3; Temp++)
+ {
+ MotorData[Temp].MotorSetSpeed = 0;
+ MotorData[Temp].MotorTargetSpeed = 0;
+ MotorData[Temp].MotorActualSpeed = 0;
+ MotorData[Temp].MotorRampUpCount = 0;
+ MotorData[Temp].MotorRampDownCount = 0;
+ MotorData[Temp].MotorRunState = 0;
+ MotorData[Temp].MotorTachoCountToRun = 0;
+ MotorData[Temp].MotorRunForever = 1;
+ MotorData[Temp].AccError = 0;
+ MotorData[Temp].RegulationTimeCount = 0;
+ MotorData[Temp].RegPParameter = DEFAULT_P_GAIN_FACTOR;
+ MotorData[Temp].RegIParameter = DEFAULT_I_GAIN_FACTOR;
+ MotorData[Temp].RegDParameter = DEFAULT_D_GAIN_FACTOR;
+ MotorData[Temp].RegulationMode = 0;
+ MotorData[Temp].MotorOverloaded = 0;
+ INSERTMode(Temp, COAST_MOTOR_MODE);
+ INSERTSpeed(Temp, MotorData[Temp].MotorSetSpeed);
+ }
+}
+
+/* This function is called every 1 mS and will go through all the motors and there 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 */
+/* MotorBlockTachoCount tell current position within current movement. Reset when a new block is started from the VM */
+/* RotationCaptureCount is additional counter for the rotationsensor. Uses it own value so it does conflict with other CaptureCount */
+void dOutputCtrl(void)
+{
+ UBYTE MotorNr;
+ SLONG NewTachoCount[3];
+
+ TACHOCaptureReadResetAll(NewTachoCount[MOTOR_A], NewTachoCount[MOTOR_B], NewTachoCount[MOTOR_C]);
+
+ for (MotorNr = 0; MotorNr < 3; MotorNr++)
+ {
+ MotorData[MotorNr].DeltaCaptureCount += NewTachoCount[MotorNr];
+ MotorData[MotorNr].CurrentCaptureCount += NewTachoCount[MotorNr];
+ MotorData[MotorNr].MotorBlockTachoCount += NewTachoCount[MotorNr];
+ MotorData[MotorNr].RotationCaptureCount += NewTachoCount[MotorNr];
+ MotorData[MotorNr].RegulationTimeCount++;
+
+ if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RAMPUP)
+ {
+ dOutputRampUpFunction(MotorNr);
+ }
+ if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RAMPDOWN)
+ {
+ dOutputRampDownFunction(MotorNr);
+ }
+ if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RUNNING)
+ {
+ dOutputTachoLimitControl(MotorNr);
+ }
+ if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_IDLE)
+ {
+ dOutputMotorIdleControl(MotorNr);
+ }
+ if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_HOLD)
+ {
+ MotorData[MotorNr].MotorSetSpeed = 0;
+ MotorData[MotorNr].MotorActualSpeed = 0;
+ MotorData[MotorNr].MotorTargetSpeed = 0;
+ MotorData[MotorNr].RegulationTimeCount = 0;
+ MotorData[MotorNr].DeltaCaptureCount = 0;
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_RUNNING;
+
+ }
+ if (MotorData[MotorNr].RegulationTimeCount > REGULATION_TIME)
+ {
+ MotorData[MotorNr].RegulationTimeCount = 0;
+ dOutputRegulateMotor(MotorNr);
+ MotorData[MotorNr].DeltaCaptureCount = 0;
+ }
+ }
+ INSERTSpeed(MOTOR_A, MotorData[MOTOR_A].MotorActualSpeed);
+ INSERTSpeed(MOTOR_B, MotorData[MOTOR_B].MotorActualSpeed);
+ INSERTSpeed(MOTOR_C, MotorData[MOTOR_C].MotorActualSpeed);
+}
+
+void dOutputExit(void)
+{
+ OUTPUTExit;
+}
+
+/* Called eveyr 1 mS */
+/* Data mapping for controller (IO-Map is updated with these values) */
+void dOutputGetMotorParameters(UBYTE *CurrentMotorSpeed, SLONG *TachoCount, SLONG *BlockTachoCount, UBYTE *RunState, UBYTE *MotorOverloaded, SLONG *RotationCount)
+{
+ UBYTE Tmp;
+
+ for (Tmp = 0; Tmp < 3; Tmp++)
+ {
+ CurrentMotorSpeed[Tmp] = MotorData[Tmp].MotorActualSpeed;
+ TachoCount[Tmp] = MotorData[Tmp].CurrentCaptureCount;
+ BlockTachoCount[Tmp] = MotorData[Tmp].MotorBlockTachoCount;
+ RotationCount[Tmp] = MotorData[Tmp].RotationCaptureCount;
+ RunState[Tmp] = MotorData[Tmp].MotorRunState;
+ MotorOverloaded[Tmp] = MotorData[Tmp].MotorOverloaded;
+ }
+}
+
+void dOutputSetMode(UBYTE Motor, UBYTE Mode) //Set motor mode (break, Float)
+{
+ INSERTMode(Motor, Mode);
+}
+
+/* Update the regulation state for the motor */
+/* Need to reset regulation parameter depending on current status of the motor */
+/* AccError & OldPositionError used for position regulation and Sync Parameter are used for synchronization regulation */
+void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode)
+{
+ MotorData[MotorNr].RegulationMode = RegulationMode;
+
+ if ((MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED) && (MotorData[MotorNr].MotorSetSpeed == 0) && (MotorData[MotorNr].MotorRunState != MOTOR_RUN_STATE_RAMPDOWN))
+ {
+ MotorData[MotorNr].AccError = 0;
+ MotorData[MotorNr].OldPositionError = 0;
+ }
+
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ if (((MotorData[MotorNr].MotorActualSpeed == 0) || (MotorData[MotorNr].TurnParameter != 0) || (MotorData[MotorNr].TurnParameter == 0)) && (MotorData[MotorNr].MotorRunState != MOTOR_RUN_STATE_RAMPDOWN))
+ {
+ SyncData.SyncTachoDif = 0;
+
+ SyncData.SyncAccError = 0;
+ SyncData.SyncOldError = 0;
+ SyncData.SyncTurnParameter = 0;
+ }
+ }
+}
+
+/* Disable current regulation if enabled */
+void dOutputDisableRegulation(UBYTE MotorNr)
+{
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+}
+
+/* Calling this function with reset count which tell current position and which is used to tell if the wanted position is obtained */
+/* Calling this function will reset current movement of the motor if it is running */
+void dOutputResetTachoLimit(UBYTE MotorNr)
+{
+ MotorData[MotorNr].CurrentCaptureCount = 0;
+ MotorData[MotorNr].MotorTachoCountToRun = 0;
+
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ dOutputResetSyncMotors(MotorNr);
+ }
+
+ if (MotorData[MotorNr].MotorRunForever == 1)
+ {
+ MotorData[MotorNr].MotorRunForever = 0; // To ensure that we get the same functionality for all combination on motor durations
+ }
+}
+
+/* MotorBlockTachoCount tells current position in current movement. */
+/* Used within the synchronization to compare current motor position. Reset on every new movement from the VM */
+void dOutputResetBlockTachoLimit(UBYTE MotorNr)
+{
+ MotorData[MotorNr].MotorBlockTachoCount = 0;
+}
+
+/* Additional counter add to help the VM application keep track of number of rotation for the rotation sensor */
+/* This values can be reset independtly from the other tacho count values used with regulation and position control */
+void dOutputResetRotationCaptureCount(UBYTE MotorNr)
+{
+ MotorData[MotorNr].RotationCaptureCount = 0;
+}
+
+/* Can be used to set new PID values */
+void dOutputSetPIDParameters(UBYTE Motor, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter)
+{
+ MotorData[Motor].RegPParameter = NewRegPParameter;
+ MotorData[Motor].RegIParameter = NewRegIParameter;
+ MotorData[Motor].RegDParameter = NewRegDParameter;
+}
+
+/* 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)
+{
+ if (BlockTachoCntToTravel == 0)
+ {
+ MotorData[MotorNr].MotorRunForever = 1;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorRunForever = 0;
+
+ if (MotorData[MotorNr].MotorSetSpeed == 0)
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed > 0)
+ {
+ MotorData[MotorNr].MotorTachoCountToRun += BlockTachoCntToTravel;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorTachoCountToRun -= BlockTachoCntToTravel;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ MotorData[MotorNr].MotorTachoCountToRun += BlockTachoCntToTravel;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorTachoCountToRun -= BlockTachoCntToTravel;
+ }
+ }
+ }
+}
+
+/* This function is used for setting up the motor mode and motor speed */
+void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE NewTurnParameter)
+{
+ if ((MotorData[MotorNr].MotorSetSpeed != Speed) || (MotorData[MotorNr].MotorRunState != NewMotorRunState) || (NewMotorRunState == MOTOR_RUN_STATE_IDLE) || (MotorData[MotorNr].TurnParameter != NewTurnParameter))
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed == 0)
+ {
+ MotorData[MotorNr].AccError = 0;
+ MotorData[MotorNr].OldPositionError = 0;
+ MotorData[MotorNr].RegulationTimeCount = 0;
+ MotorData[MotorNr].DeltaCaptureCount = 0;
+ TACHOCountReset(MotorNr);
+ }
+ switch (NewMotorRunState)
+ {
+ case MOTOR_RUN_STATE_IDLE:
+ {
+ //MotorData[MotorNr].MotorSetSpeed = 0;
+ //MotorData[MotorNr].MotorTargetSpeed = 0;
+ //MotorData[MotorNr].TurnParameter = 0;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+ break;
+
+ case MOTOR_RUN_STATE_RAMPUP:
+ {
+ if (MotorData[MotorNr].MotorSetSpeed == 0)
+ {
+ MotorData[MotorNr].MotorSetSpeed = Speed;
+ MotorData[MotorNr].TurnParameter = NewTurnParameter;
+ MotorData[MotorNr].MotorRampUpIncrement = 0;
+ MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ }
+ else
+ {
+ if (Speed > 0)
+ {
+ if (MotorData[MotorNr].MotorSetSpeed >= Speed)
+ {
+ NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorSetSpeed = Speed;
+ MotorData[MotorNr].TurnParameter = NewTurnParameter;
+ MotorData[MotorNr].MotorRampUpIncrement = 0;
+ MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorSetSpeed <= Speed)
+ {
+ NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorSetSpeed = Speed;
+ MotorData[MotorNr].TurnParameter = NewTurnParameter;
+ MotorData[MotorNr].MotorRampUpIncrement = 0;
+ MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ }
+ }
+ }
+ }
+ break;
+
+ case MOTOR_RUN_STATE_RUNNING:
+ {
+ MotorData[MotorNr].MotorSetSpeed = Speed;
+ MotorData[MotorNr].MotorTargetSpeed = Speed;
+ MotorData[MotorNr].TurnParameter = NewTurnParameter;
+
+ if (MotorData[MotorNr].MotorSetSpeed == 0)
+ {
+ NewMotorRunState = MOTOR_RUN_STATE_HOLD;
+ }
+ }
+ break;
+
+ case MOTOR_RUN_STATE_RAMPDOWN:
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed >= 0)
+ {
+ if (MotorData[MotorNr].MotorSetSpeed <= Speed)
+ {
+ NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorSetSpeed = Speed;
+ MotorData[MotorNr].TurnParameter = NewTurnParameter;
+ MotorData[MotorNr].MotorRampDownIncrement = 0;
+ MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampDownCount = 0;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorSetSpeed >= Speed)
+ {
+ NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorSetSpeed = Speed;
+ MotorData[MotorNr].TurnParameter = NewTurnParameter;
+ MotorData[MotorNr].MotorRampDownIncrement = 0;
+ MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampDownCount = 0;
+ }
+ }
+ }
+ break;
+ }
+ MotorData[MotorNr].MotorRunState = NewMotorRunState;
+ MotorData[MotorNr].MotorOverloaded = 0;
+ }
+}
+
+/* Function used for controlling the Ramp-up periode */
+/* Ramp-up is done with 1 increment in speed every X number of TachoCount, where X depend on duration of the periode and the wanted speed */
+void dOutputRampUpFunction(UBYTE MotorNr)
+{
+ if (MotorData[MotorNr].MotorTargetSpeed == 0)
+ {
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ MotorData[MotorNr].MotorTargetSpeed = MIN_MOVEMENT_POWER;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorTargetSpeed = -MIN_MOVEMENT_POWER;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorRampUpIncrement == 0)
+ {
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ MotorData[MotorNr].MotorRampUpIncrement = (SWORD)((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart) / (MotorData[MotorNr].MotorSetSpeed - MotorData[MotorNr].MotorTargetSpeed));
+ }
+ else
+ {
+ MotorData[MotorNr].MotorRampUpIncrement = (SWORD)(-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart) / (MotorData[MotorNr].MotorSetSpeed - MotorData[MotorNr].MotorTargetSpeed)));
+ }
+ MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
+ }
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount > (MotorData[MotorNr].MotorRampTachoCountOld + MotorData[MotorNr].MotorRampUpIncrement))
+ {
+ MotorData[MotorNr].MotorTargetSpeed += 1;
+ MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ }
+ else
+ {
+ if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
+ {
+ MotorData[MotorNr].MotorRampUpCount++;
+ if (MotorData[MotorNr].MotorRampUpCount > 100)
+ {
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ MotorData[MotorNr].MotorTargetSpeed++;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount < (MotorData[MotorNr].MotorRampTachoCountOld + MotorData[MotorNr].MotorRampUpIncrement))
+ {
+ MotorData[MotorNr].MotorTargetSpeed -= 1;
+ MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ }
+ else
+ {
+ if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
+ {
+ MotorData[MotorNr].MotorRampUpCount++;
+ if (MotorData[MotorNr].MotorRampUpCount > 100)
+ {
+ MotorData[MotorNr].MotorRampUpCount = 0;
+ MotorData[MotorNr].MotorTargetSpeed--;
+ }
+ }
+ }
+ }
+ }
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ if ((MotorData[MotorNr].CurrentCaptureCount - MotorData[MotorNr].MotorRampTachoCountStart) >= (MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart))
+ {
+ MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ }
+ }
+ else
+ {
+ if ((MotorData[MotorNr].CurrentCaptureCount + MotorData[MotorNr].MotorRampTachoCountStart) <= (MotorData[MotorNr].MotorTachoCountToRun + MotorData[MotorNr].MotorRampTachoCountStart))
+ {
+ MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ }
+ }
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed > MotorData[MotorNr].MotorSetSpeed)
+ {
+ MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed < MotorData[MotorNr].MotorSetSpeed)
+ {
+ MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
+ }
+ }
+ if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE)
+ {
+ MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed;
+ }
+}
+
+/* Function used for controlling the Ramp-down periode */
+/* Ramp-down is done with 1 decrement in speed every X number of TachoCount, where X depend on duration of the periode and the wanted speed */
+void dOutputRampDownFunction(UBYTE MotorNr)
+{
+ if (MotorData[MotorNr].MotorRampDownIncrement == 0)
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed > 0)
+ {
+ if ((MotorData[MotorNr].MotorTargetSpeed > MIN_MOVEMENT_POWER) && (MotorData[MotorNr].MotorSetSpeed == 0))
+ {
+ MotorData[MotorNr].MotorRampDownIncrement = ((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / ((MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed) - MIN_MOVEMENT_POWER));
+ }
+ else
+ {
+ MotorData[MotorNr].MotorRampDownIncrement = ((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / (MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed));
+ }
+ }
+ else
+ {
+ if ((MotorData[MotorNr].MotorTargetSpeed < -MIN_MOVEMENT_POWER) && (MotorData[MotorNr].MotorSetSpeed == 0))
+ {
+ MotorData[MotorNr].MotorRampDownIncrement = (-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / ((MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed) + MIN_MOVEMENT_POWER)));
+ }
+ else
+ {
+ MotorData[MotorNr].MotorRampDownIncrement = (-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / (MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed)));
+ }
+ }
+ MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
+ }
+ if (MotorData[MotorNr].MotorTargetSpeed > 0)
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount > (MotorData[MotorNr].MotorRampTachoCountOld + (SLONG)MotorData[MotorNr].MotorRampDownIncrement))
+ {
+ MotorData[MotorNr].MotorTargetSpeed--;
+ if (MotorData[MotorNr].MotorTargetSpeed < MIN_MOVEMENT_POWER)
+ {
+ MotorData[MotorNr].MotorTargetSpeed = MIN_MOVEMENT_POWER;
+ }
+ MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampDownCount = 0;
+ dOutputRampDownSynch(MotorNr);
+ }
+ else
+ {
+ if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
+ {
+ MotorData[MotorNr].MotorRampDownCount++;
+ if (MotorData[MotorNr].MotorRampDownCount > (UWORD)(30 * MotorData[MotorNr].MotorRampDownIncrement))
+ {
+ MotorData[MotorNr].MotorRampDownCount = (UWORD)(20 * MotorData[MotorNr].MotorRampDownIncrement);
+ MotorData[MotorNr].MotorTargetSpeed++;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount < (MotorData[MotorNr].MotorRampTachoCountOld + (SLONG)MotorData[MotorNr].MotorRampDownIncrement))
+ {
+ MotorData[MotorNr].MotorTargetSpeed++;
+ if (MotorData[MotorNr].MotorTargetSpeed > -MIN_MOVEMENT_POWER)
+ {
+ MotorData[MotorNr].MotorTargetSpeed = -MIN_MOVEMENT_POWER;
+ }
+ MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
+ MotorData[MotorNr].MotorRampDownCount = 0;
+ dOutputRampDownSynch(MotorNr);
+ }
+ else
+ {
+ if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
+ {
+ MotorData[MotorNr].MotorRampDownCount++;
+ if (MotorData[MotorNr].MotorRampDownCount > (UWORD)(30 * (-MotorData[MotorNr].MotorRampDownIncrement)))
+ {
+ MotorData[MotorNr].MotorRampDownCount = (UWORD)(20 * (-MotorData[MotorNr].MotorRampDownIncrement));
+ MotorData[MotorNr].MotorTargetSpeed--;
+ }
+ }
+ }
+ }
+ if ((MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) && (MotorData[MotorNr].TurnParameter != 0))
+ {
+ dOutputSyncTachoLimitControl(MotorNr);
+ if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_IDLE)
+ {
+ dOutputMotorReachedTachoLimit(MotorNr);
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed > 0)
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun)
+ {
+ dOutputMotorReachedTachoLimit(MotorNr);
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun)
+ {
+ dOutputMotorReachedTachoLimit(MotorNr);
+ }
+ }
+ }
+ if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE)
+ {
+ MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed;
+ }
+}
+
+/* Function used to tell whether the wanted position is obtained */
+void dOutputTachoLimitControl(UBYTE MotorNr)
+{
+ if (MotorData[MotorNr].MotorRunForever == 0)
+ {
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ dOutputSyncTachoLimitControl(MotorNr);
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ if ((MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun))
+ {
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorSetSpeed < 0)
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun)
+ {
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].CurrentCaptureCount > MAX_COUNT_TO_RUN)
+ {
+ MotorData[MotorNr].CurrentCaptureCount = 0;
+ }
+ if (MotorData[MotorNr].MotorTargetSpeed != 0)
+ {
+ MotorData[MotorNr].MotorTachoCountToRun = MotorData[MotorNr].CurrentCaptureCount;
+ }
+ }
+ if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE)
+ {
+ MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed;
+ }
+}
+
+/* Function used to decrease speed slowly when the motor is set to idle */
+void dOutputMotorIdleControl(UBYTE MotorNr)
+{
+ INSERTMode(MotorNr, COAST_MOTOR_MODE);
+
+ if (MotorData[MotorNr].MotorActualSpeed != 0)
+ {
+ if (MotorData[MotorNr].MotorActualSpeed > 0)
+ {
+ MotorData[MotorNr].MotorActualSpeed--;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorActualSpeed++;
+ }
+ }
+
+ if (MotorData[MotorNr].MotorTargetSpeed != 0)
+ {
+ if (MotorData[MotorNr].MotorTargetSpeed > 0)
+ {
+ MotorData[MotorNr].MotorTargetSpeed--;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorTargetSpeed++;
+ }
+ }
+
+ if (MotorData[MotorNr].MotorSetSpeed != 0)
+ {
+ if (MotorData[MotorNr].MotorSetSpeed > 0)
+ {
+ MotorData[MotorNr].MotorSetSpeed--;
+ }
+ else
+ {
+ MotorData[MotorNr].MotorSetSpeed++;
+ }
+ }
+}
+
+/* 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)
+{
+ UBYTE SyncMotorOne;
+ UBYTE SyncMotorTwo;
+
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)
+ {
+ dOutputCalculateMotorPosition(MotorNr);
+ }
+ else
+ {
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ dOutputMotorSyncStatus(MotorNr, &SyncMotorOne, &SyncMotorTwo);
+
+ if ((SyncMotorOne != 0xFF) &&(SyncMotorTwo != 0xFF))
+ {
+ dOutputSyncMotorPosition(SyncMotorOne, SyncMotorTwo);
+ }
+ }
+ }
+}
+
+/* Regulation function used when Position regulation is enabled */
+/* 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;
+
+ NewSpeedCount = (SWORD)((MotorData[MotorNr].MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);
+
+ PositionError = (SWORD)(MotorData[MotorNr].OldPositionError - MotorData[MotorNr].DeltaCaptureCount) + NewSpeedCount;
+
+ //Overflow control on PositionError
+ if (MotorData[MotorNr].RegPParameter != 0)
+ {
+ if (PositionError > (SWORD)(32000 / MotorData[MotorNr].RegPParameter))
+ {
+ PositionError = (SWORD)(32000 / MotorData[MotorNr].RegPParameter);
+ }
+ if (PositionError < (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter)))
+ {
+ PositionError = (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter));
+ }
+ }
+ else
+ {
+ if (PositionError > (SWORD)32000)
+ {
+ PositionError = (SWORD)32000;
+ }
+ if (PositionError < (SWORD)-32000)
+ {
+ PositionError = (SWORD)-32000;
+ }
+ }
+
+ PValue = PositionError * (SWORD)(MotorData[MotorNr].RegPParameter/REG_CONST_DIV);
+ if (PValue > (SWORD)REG_MAX_VALUE)
+ {
+ PValue = REG_MAX_VALUE;
+ }
+ if (PValue <= (SWORD)REG_MIN_VALUE)
+ {
+ PValue = REG_MIN_VALUE;
+ }
+
+ DValue = (PositionError - MotorData[MotorNr].OldPositionError) * (SWORD)(MotorData[MotorNr].RegDParameter/REG_CONST_DIV);
+ MotorData[MotorNr].OldPositionError = PositionError;
+
+ MotorData[MotorNr].AccError = (MotorData[MotorNr].AccError * 3) + PositionError;
+ MotorData[MotorNr].AccError = MotorData[MotorNr].AccError / 4;
+
+ if (MotorData[MotorNr].AccError > (SWORD)800)
+ {
+ MotorData[MotorNr].AccError = 800;
+ }
+ if (MotorData[MotorNr].AccError <= (SWORD)-800)
+ {
+ MotorData[MotorNr].AccError = -800;
+ }
+ IValue = MotorData[MotorNr].AccError * (SWORD)(MotorData[MotorNr].RegIParameter/REG_CONST_DIV);
+
+ 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);
+
+ if (TotalRegValue > MAXIMUM_SPEED_FW)
+ {
+ TotalRegValue = MAXIMUM_SPEED_FW;
+ MotorData[MotorNr].MotorOverloaded = 1;
+ }
+ if (TotalRegValue < MAXIMUM_SPEED_RW)
+ {
+ TotalRegValue = MAXIMUM_SPEED_RW;
+ MotorData[MotorNr].MotorOverloaded = 1;
+ }
+ MotorData[MotorNr].MotorActualSpeed = (SBYTE)TotalRegValue;
+}
+
+/* Regulation function used when syncrhonization regulation is enabled */
+/* The regulation form controls two motors at a time */
+void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
+{
+ SLONG TempTurnParameter;
+ SWORD PValue;
+ SWORD IValue;
+ SWORD DValue;
+ SWORD CorrectionValue;
+ SWORD MotorSpeed;
+
+ SyncData.SyncTachoDif = (SLONG)((MotorData[MotorOne].MotorBlockTachoCount) - (MotorData[MotorTwo].MotorBlockTachoCount));
+
+ if (MotorData[MotorOne].TurnParameter != 0)
+ {
+ if ((MotorData[MotorOne].MotorBlockTachoCount != 0) || (MotorData[MotorTwo].MotorBlockTachoCount))
+ {
+ if (MotorData[MotorOne].MotorTargetSpeed >= 0)
+ {
+ if (MotorData[MotorOne].TurnParameter > 0)
+ {
+ TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorTwo].TurnParameter * (SLONG)MotorData[MotorTwo].MotorTargetSpeed)/100);
+ }
+ else
+ {
+ TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorOne].TurnParameter * (SLONG)MotorData[MotorOne].MotorTargetSpeed)/100);
+ }
+ }
+ else
+ {
+ if (MotorData[MotorOne].TurnParameter > 0)
+ {
+ TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorOne].TurnParameter * (-(SLONG)MotorData[MotorOne].MotorTargetSpeed))/100);
+ }
+ else
+ {
+ TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorTwo].TurnParameter * (-(SLONG)MotorData[MotorTwo].MotorTargetSpeed))/100);
+ }
+ }
+ }
+ else
+ {
+ TempTurnParameter = MotorData[MotorOne].TurnParameter;
+ }
+ }
+ else
+ {
+ TempTurnParameter = 0;
+ }
+
+ SyncData.SyncTurnParameter += (SLONG)(((TempTurnParameter * (MAX_CAPTURE_COUNT))/INPUT_SCALE_FACTOR)*2);
+ //SyncTurnParameter should ophold difference between the two motors.
+
+ SyncData.SyncTachoDif += SyncData.SyncTurnParameter;
+
+ 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)(MotorData[MotorOne].RegPParameter/REG_CONST_DIV);
+
+ DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(MotorData[MotorOne].RegDParameter/REG_CONST_DIV);
+ SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif;
+
+ SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif;
+
+ if (SyncData.SyncAccError > (SWORD)900)
+ {
+ SyncData.SyncAccError = 900;
+ }
+ if (SyncData.SyncAccError < (SWORD)-900)
+ {
+ SyncData.SyncAccError = -900;
+ }
+ IValue = SyncData.SyncAccError * (SWORD)(MotorData[MotorOne].RegIParameter/REG_CONST_DIV);
+
+ CorrectionValue = (SWORD)((PValue + IValue + DValue)/4);
+
+ MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed - CorrectionValue;
+
+ if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
+ {
+ MotorSpeed = MAXIMUM_SPEED_FW;
+ }
+ else
+ {
+ if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
+ {
+ MotorSpeed = MAXIMUM_SPEED_RW;
+ }
+ }
+
+ if (MotorData[MotorOne].TurnParameter != 0)
+ {
+ if (MotorData[MotorOne].MotorTargetSpeed > 0)
+ {
+ if (MotorSpeed > (SWORD)MotorData[MotorOne].MotorTargetSpeed)
+ {
+ MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed;
+ }
+ else
+ {
+ if (MotorSpeed < (SWORD)-MotorData[MotorOne].MotorTargetSpeed)
+ {
+ MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed;
+ }
+ }
+ }
+ else
+ {
+ if (MotorSpeed < (SWORD)MotorData[MotorOne].MotorTargetSpeed)
+ {
+ MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed;
+ }
+ else
+ {
+ if (MotorSpeed > (SWORD)-MotorData[MotorOne].MotorTargetSpeed)
+ {
+ MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed;
+ }
+ }
+ }
+ }
+ MotorData[MotorOne].MotorActualSpeed = (SBYTE)MotorSpeed;
+
+ MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed + CorrectionValue;
+
+ if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
+ {
+ MotorSpeed = MAXIMUM_SPEED_FW;
+ }
+ else
+ {
+ if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
+ {
+ MotorSpeed = MAXIMUM_SPEED_RW;
+ }
+ }
+
+ if (MotorData[MotorOne].TurnParameter != 0)
+ {
+ if (MotorData[MotorTwo].MotorTargetSpeed > 0)
+ {
+ if (MotorSpeed > (SWORD)MotorData[MotorTwo].MotorTargetSpeed)
+ {
+ MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed;
+ }
+ else
+ {
+ if (MotorSpeed < (SWORD)-MotorData[MotorTwo].MotorTargetSpeed)
+ {
+ MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed;
+ }
+ }
+ }
+ else
+ {
+ if (MotorSpeed < (SWORD)MotorData[MotorTwo].MotorTargetSpeed)
+ {
+ MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed;
+ }
+ else
+ {
+ if (MotorSpeed > (SWORD)-MotorData[MotorTwo].MotorTargetSpeed)
+ {
+ MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed;
+ }
+ }
+ }
+ }
+ MotorData[MotorTwo].MotorActualSpeed = (SBYTE)MotorSpeed;
+}
+
+//Called when the motor is ramping down
+void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
+{
+ UBYTE MotorOne, MotorTwo;
+
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ if (MotorNr == MOTOR_A)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ MotorData[MotorOne].MotorSetSpeed = 0;
+ MotorData[MotorOne].MotorTargetSpeed = 0;
+ MotorData[MotorOne].MotorActualSpeed = 0;
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
+ MotorData[MotorTwo].MotorSetSpeed = 0;
+ MotorData[MotorTwo].MotorTargetSpeed = 0;
+ MotorData[MotorTwo].MotorActualSpeed = 0;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
+ }
+ else
+ {
+ MotorTwo = MotorOne + 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ MotorData[MotorOne].MotorSetSpeed = 0;
+ MotorData[MotorOne].MotorTargetSpeed = 0;
+ MotorData[MotorOne].MotorActualSpeed = 0;
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
+ MotorData[MotorTwo].MotorSetSpeed = 0;
+ MotorData[MotorTwo].MotorTargetSpeed = 0;
+ MotorData[MotorTwo].MotorActualSpeed = 0;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
+ }
+ else
+ {
+ //Only Motor A has Sync setting => Stop normal
+ MotorData[MotorNr].MotorSetSpeed = 0;
+ MotorData[MotorNr].MotorTargetSpeed = 0;
+ MotorData[MotorNr].MotorActualSpeed = 0;
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_B)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ MotorData[MotorOne].MotorSetSpeed = 0;
+ MotorData[MotorOne].MotorTargetSpeed = 0;
+ MotorData[MotorOne].MotorActualSpeed = 0;
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
+ MotorData[MotorTwo].MotorSetSpeed = 0;
+ MotorData[MotorTwo].MotorTargetSpeed = 0;
+ MotorData[MotorTwo].MotorActualSpeed = 0;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
+ }
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C
+ MotorData[MotorOne].MotorSetSpeed = 0;
+ MotorData[MotorOne].MotorTargetSpeed = 0;
+ MotorData[MotorOne].MotorActualSpeed = 0;
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
+ MotorData[MotorTwo].MotorSetSpeed = 0;
+ MotorData[MotorTwo].MotorTargetSpeed = 0;
+ MotorData[MotorTwo].MotorActualSpeed = 0;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
+ }
+ else
+ {
+ //Only Motor B has Sync settings => Stop normal
+ MotorData[MotorNr].MotorSetSpeed = 0;
+ MotorData[MotorNr].MotorTargetSpeed = 0;
+ MotorData[MotorNr].MotorActualSpeed = 0;
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+ }
+ if (MotorNr == MOTOR_C)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ MotorData[MotorOne].MotorSetSpeed = 0;
+ MotorData[MotorOne].MotorTargetSpeed = 0;
+ MotorData[MotorOne].MotorActualSpeed = 0;
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
+ MotorData[MotorTwo].MotorSetSpeed = 0;
+ MotorData[MotorTwo].MotorTargetSpeed = 0;
+ MotorData[MotorTwo].MotorActualSpeed = 0;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
+ }
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C
+ MotorData[MotorOne].MotorSetSpeed = 0;
+ MotorData[MotorOne].MotorTargetSpeed = 0;
+ MotorData[MotorOne].MotorActualSpeed = 0;
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
+ MotorData[MotorTwo].MotorSetSpeed = 0;
+ MotorData[MotorTwo].MotorTargetSpeed = 0;
+ MotorData[MotorTwo].MotorActualSpeed = 0;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE;
+ }
+ else
+ {
+ //Only Motor C has Sync settings => Stop normal
+ MotorData[MotorNr].MotorSetSpeed = 0;
+ MotorData[MotorNr].MotorTargetSpeed = 0;
+ MotorData[MotorNr].MotorActualSpeed = 0;
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorNr].MotorSetSpeed == 0)
+ {
+ MotorData[MotorNr].MotorSetSpeed = 0;
+ MotorData[MotorNr].MotorTargetSpeed = 0;
+ MotorData[MotorNr].MotorActualSpeed = 0;
+ }
+ MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
+ }
+}
+
+/* Function used for control tacho limit when motors are synchronised */
+/* Special control is needed when the motor are turning */
+void dOutputSyncTachoLimitControl(UBYTE MotorNr)
+{
+ UBYTE MotorOne, MotorTwo;
+
+ if (MotorNr == MOTOR_A)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ }
+ else
+ {
+ MotorTwo = MotorOne + 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ }
+ else
+ {
+ //Only Motor A has Sync setting => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_B)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B, which has already been called when running throught motor A
+ //MotorOne = 0xFF;
+ //MotorTwo = 0xFF;
+ }
+ else
+ {
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C
+ }
+ else
+ {
+ //Only Motor B has Sync settings => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_C)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C, which has already been called when running throught motor A
+ //MotorOne = 0xFF;
+ //MotorTwo = 0xFF;
+ }
+ else
+ {
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C, which has already been called when running throught motor B
+ //MotorOne = 0xFF;
+ //MotorTwo = 0xFF;
+ }
+ else
+ {
+ //Only Motor C has Sync settings => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+
+ if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
+ {
+ if (MotorData[MotorOne].TurnParameter != 0)
+ {
+ if (MotorData[MotorOne].TurnParameter > 0)
+ {
+ if (MotorData[MotorTwo].MotorTargetSpeed >= 0)
+ {
+ if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun))
+ {
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+
+ MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount;
+ MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun;
+ }
+ }
+ else
+ {
+ if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun))
+ {
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+
+ MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount;
+ MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun;
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorOne].MotorTargetSpeed >= 0)
+ {
+ if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun))
+ {
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+
+ MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount;
+ MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun;
+ }
+ }
+ else
+ {
+ if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun))
+ {
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+
+ MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount;
+ MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorOne].MotorSetSpeed > 0)
+ {
+ if ((MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun) || (MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun))
+ {
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorOne].MotorSetSpeed < 0)
+ {
+ if ((MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun) || (MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun))
+ {
+ MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
+ }
+ }
+ }
+ }
+ }
+}
+
+/* Function which can evaluate which motor are synched */
+void dOutputMotorSyncStatus(UBYTE MotorNr, UBYTE *SyncMotorOne, UBYTE *SyncMotorTwo)
+{
+ if (MotorNr < MOTOR_C)
+ {
+ if (MotorNr == MOTOR_A)
+ {
+ *SyncMotorOne = MotorNr;
+ *SyncMotorTwo = *SyncMotorOne + 1;
+ if (MotorData[*SyncMotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ }
+ else
+ {
+ *SyncMotorTwo = *SyncMotorOne + 2;
+ if (MotorData[*SyncMotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ }
+ else
+ {
+ //Only Motor A has Sync setting => Do nothing, treat motor as motor without regulation
+ *SyncMotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_B)
+ {
+ *SyncMotorOne = MotorNr;
+ *SyncMotorTwo = *SyncMotorOne + 1;
+ if (MotorData[*SyncMotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ if (!(MotorData[MOTOR_A].RegulationMode & REGSTATE_SYNCHRONE))
+ {
+ //Synchronise motor B & C
+ }
+ }
+ else
+ {
+ //Only Motor B has Sync settings or Motor is sync. with Motor A and has therefore already been called
+ *SyncMotorTwo = 0xFF;
+ }
+ }
+ }
+ else
+ {
+ *SyncMotorOne = 0xFF;
+ *SyncMotorTwo = 0xFF;
+ }
+}
+/* Function which is called when motors are synchronized and the motor position is reset */
+void dOutputResetSyncMotors(UBYTE MotorNr)
+{
+ UBYTE MotorOne, MotorTwo;
+
+ if (MotorNr == MOTOR_A)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ }
+ else
+ {
+ MotorTwo = MotorOne + 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ }
+ else
+ {
+ //Only Motor A has Sync setting => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_B)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ }
+ else
+ {
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C
+ }
+ else
+ {
+ //Only Motor B has Sync settings => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_C)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ }
+ else
+ {
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C
+ }
+ else
+ {
+ //Only Motor C has Sync settings => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+
+ if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
+ {
+ MotorData[MotorOne].CurrentCaptureCount = 0;
+ MotorData[MotorOne].MotorTachoCountToRun = 0;
+ MotorData[MotorTwo].CurrentCaptureCount = 0;
+ MotorData[MotorTwo].MotorTachoCountToRun = 0;
+ }
+ else
+ {
+ MotorData[MotorNr].CurrentCaptureCount = 0;
+ MotorData[MotorNr].MotorTachoCountToRun = 0;
+ }
+}
+
+/* Function which is called when motors are synchronized and motor is ramping down */
+void dOutputRampDownSynch(UBYTE MotorNr)
+{
+ UBYTE MotorOne, MotorTwo;
+
+ if (MotorNr == MOTOR_A)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B
+ }
+ else
+ {
+ MotorTwo = MotorOne + 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C
+ }
+ else
+ {
+ //Only Motor A has Sync setting => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_B)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & B, which has already been called when running throught motor A
+ //MotorOne = 0xFF;
+ //MotorTwo = 0xFF;
+ }
+ else
+ {
+ MotorTwo = MotorOne + 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C
+ }
+ else
+ {
+ //Only Motor B has Sync settings => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+ if (MotorNr == MOTOR_C)
+ {
+ MotorOne = MotorNr;
+ MotorTwo = MotorOne - 2;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor A & C, which has already been called when running throught motor A
+ }
+ else
+ {
+ MotorTwo = MotorOne - 1;
+ if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
+ {
+ //Synchronise motor B & C,, which has already been called when running throught motor B
+ }
+ else
+ {
+ //Only Motor C has Sync settings => Stop normal
+ MotorOne = 0xFF;
+ MotorTwo = 0xFF;
+ }
+ }
+ }
+
+ if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
+ {
+ if (MotorData[MotorOne].TurnParameter != 0)
+ {
+ if (MotorData[MotorOne].TurnParameter > 0)
+ {
+ if (MotorData[MotorOne].MotorTargetSpeed >= 0)
+ {
+ if (MotorData[MotorTwo].MotorActualSpeed < 0)
+ {
+ MotorData[MotorTwo].MotorTargetSpeed--;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorTwo].MotorActualSpeed > 0)
+ {
+ MotorData[MotorTwo].MotorTargetSpeed++;
+ }
+ }
+ }
+ else
+ {
+ if (MotorData[MotorOne].MotorTargetSpeed >= 0)
+ {
+ if (MotorData[MotorTwo].MotorActualSpeed < 0)
+ {
+ MotorData[MotorTwo].MotorTargetSpeed--;
+ }
+ }
+ else
+ {
+ if (MotorData[MotorTwo].MotorActualSpeed > 0)
+ {
+ MotorData[MotorTwo].MotorTargetSpeed++;
+ }
+ }
+ }
+ }
+ }
+}
+