aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source/d_output.c
diff options
context:
space:
mode:
authorNicolas Schodet2023-03-06 17:36:31 +0100
committerNicolas Schodet2023-03-06 17:36:31 +0100
commitd383f2bcdb6ff13cc562fce1ff55d826035debad (patch)
treef5cd0440869b8ba823fd254b361cdefe95e153b4 /AT91SAM7S256/Source/d_output.c
parent8fce63fda48a6593870ffd3c584c9dd0808bc2c2 (diff)
Simplify source tree
Now just use make in the root directory to build.
Diffstat (limited to 'AT91SAM7S256/Source/d_output.c')
-rw-r--r--AT91SAM7S256/Source/d_output.c1415
1 files changed, 0 insertions, 1415 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
deleted file mode 100644
index 326f6f8..0000000
--- a/AT91SAM7S256/Source/d_output.c
+++ /dev/null
@@ -1,1415 +0,0 @@
-//
-// 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"
-
-#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 RAMPDOWN_STATE_RAMPDOWN 0
-#define RAMPDOWN_STATE_CONTINIUE 1
-
-#define COAST_MOTOR_MODE 0
-
-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))
-
-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
- 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
- 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
- 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
-}MOTORDATA;
-
-typedef struct
-{
- SLONG SyncTachoDif;
- SLONG SyncTurnParameter;
- SWORD SyncOldError;
- SWORD SyncAccError;
-}SYNCMOTORDATA;
-
-static MOTORDATA MotorData[3];
-static SYNCMOTORDATA SyncData;
-static UBYTE RegulationTime;
-static UBYTE RegulationOptions;
-
-void dOutputInit(void)
-{
- UBYTE Temp;
-
- OUTPUTInit;
- ENABLECaptureMotorA;
- ENABLECaptureMotorB;
- ENABLECaptureMotorC;
-
- for (Temp = 0; Temp < 3; Temp++)
- {
- MOTORDATA * pMD = &(MotorData[Temp]);
- pMD->MotorSetSpeed = 0;
- pMD->MotorTargetSpeed = 0;
- pMD->MotorActualSpeed = 0;
- pMD->MotorRampUpCount = 0;
- pMD->MotorRampDownCount = 0;
- pMD->MotorRunState = 0;
- 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;
- 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 */
-/* 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 * pMD = &(MotorData[MotorNr]);
- pMD->DeltaCaptureCount += NewTachoCount[MotorNr];
- pMD->CurrentCaptureCount += NewTachoCount[MotorNr];
- pMD->MotorBlockTachoCount += NewTachoCount[MotorNr];
- pMD->RotationCaptureCount += NewTachoCount[MotorNr];
- pMD->RegulationTimeCount++;
-
- if (pMD->MotorRunState == MOTOR_RUN_STATE_RAMPUP)
- {
- dOutputRampUpFunction(MotorNr);
- }
- if (pMD->MotorRunState == MOTOR_RUN_STATE_RAMPDOWN)
- {
- dOutputRampDownFunction(MotorNr);
- }
- if (pMD->MotorRunState == MOTOR_RUN_STATE_RUNNING)
- {
- dOutputTachoLimitControl(MotorNr);
- }
- if (pMD->MotorRunState == MOTOR_RUN_STATE_IDLE)
- {
- dOutputMotorIdleControl(MotorNr);
- }
- if (pMD->MotorRunState == MOTOR_RUN_STATE_HOLD)
- {
- 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 > RegulationTime)
- {
- pMD->RegulationTimeCount = 0;
- dOutputRegulateMotor(MotorNr);
- pMD->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++)
- {
- 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;
- }
-}
-
-void dOutputSetMode(UBYTE MotorNr, UBYTE Mode) //Set motor mode (break, Float)
-{
- INSERTMode(MotorNr, 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 * pMD = &(MotorData[MotorNr]);
- pMD->RegulationMode = RegulationMode;
-
- if ((pMD->RegulationMode & REGSTATE_REGULATED) && (pMD->MotorSetSpeed == 0) && (pMD->MotorRunState != MOTOR_RUN_STATE_RAMPDOWN))
- {
- pMD->AccError = 0;
- pMD->OldPositionError = 0;
- pMD->PositionFracError = 0;
- }
-
- if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
- {
- if (((pMD->MotorActualSpeed == 0) || (pMD->TurnParameter != 0) || (pMD->TurnParameter == 0)) && (pMD->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 * pMD = &(MotorData[MotorNr]);
- pMD->CurrentCaptureCount = 0;
- pMD->MotorTachoCountToRun = 0;
- pMD->MotorTachoCountTarget = 0;
-
- if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
- {
- dOutputResetSyncMotors(MotorNr);
- }
-
- if (pMD->MotorRunForever == 1)
- {
- pMD->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 MotorNr, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter)
-{
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- pMD->RegPParameter = NewRegPParameter;
- pMD->RegIParameter = NewRegIParameter;
- 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)
-{
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (pMD->RegulationMode & REGSTATE_POSITION)
- {
- pMD->MotorRunForever = 0;
- pMD->MotorTachoCountToRun = BlockTachoCntToTravel;
- }
- else if (BlockTachoCntToTravel == 0)
- {
- pMD->MotorRunForever = 1;
- }
- else
- {
- pMD->MotorRunForever = 0;
-
- if (pMD->MotorSetSpeed == 0)
- {
- if (pMD->MotorTargetSpeed > 0)
- {
- pMD->MotorTachoCountToRun += BlockTachoCntToTravel;
- }
- else
- {
- pMD->MotorTachoCountToRun -= BlockTachoCntToTravel;
- }
- }
- else
- {
- if (pMD->MotorSetSpeed > 0)
- {
- pMD->MotorTachoCountToRun += BlockTachoCntToTravel;
- }
- else
- {
- pMD->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)
-{
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- if ((pMD->MotorSetSpeed != Speed) || (pMD->MotorRunState != NewMotorRunState) ||
- (NewMotorRunState == MOTOR_RUN_STATE_IDLE) || (pMD->TurnParameter != NewTurnParameter))
- {
- if (pMD->MotorTargetSpeed == 0)
- {
- pMD->AccError = 0;
- pMD->OldPositionError = 0;
- pMD->PositionFracError = 0;
- pMD->RegulationTimeCount = 0;
- pMD->DeltaCaptureCount = 0;
- TACHOCountReset(MotorNr);
- }
- switch (NewMotorRunState)
- {
- case MOTOR_RUN_STATE_IDLE:
- {
- //pMD->MotorSetSpeed = 0;
- //pMD->MotorTargetSpeed = 0;
- //pMD->TurnParameter = 0;
- pMD->RegulationMode = REGSTATE_IDLE;
- }
- break;
-
- case MOTOR_RUN_STATE_RAMPUP:
- {
- if (pMD->MotorSetSpeed == 0)
- {
- pMD->MotorSetSpeed = Speed;
- pMD->TurnParameter = NewTurnParameter;
- pMD->MotorRampUpIncrement = 0;
- pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount;
- pMD->MotorRampUpCount = 0;
- }
- else
- {
- if (Speed > 0)
- {
- if (pMD->MotorSetSpeed >= Speed)
- {
- NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
- }
- else
- {
- pMD->MotorSetSpeed = Speed;
- pMD->TurnParameter = NewTurnParameter;
- pMD->MotorRampUpIncrement = 0;
- pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount;
- pMD->MotorRampUpCount = 0;
- }
- }
- else
- {
- if (pMD->MotorSetSpeed <= Speed)
- {
- NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
- }
- else
- {
- pMD->MotorSetSpeed = Speed;
- pMD->TurnParameter = NewTurnParameter;
- pMD->MotorRampUpIncrement = 0;
- pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount;
- pMD->MotorRampUpCount = 0;
- }
- }
- }
- }
- break;
-
- case MOTOR_RUN_STATE_RUNNING:
- {
- pMD->MotorSetSpeed = Speed;
- pMD->MotorTargetSpeed = Speed;
- pMD->TurnParameter = NewTurnParameter;
-
- if (pMD->MotorSetSpeed == 0)
- {
- NewMotorRunState = MOTOR_RUN_STATE_HOLD;
- }
- }
- break;
-
- case MOTOR_RUN_STATE_RAMPDOWN:
- {
- if (pMD->MotorTargetSpeed >= 0)
- {
- if (pMD->MotorSetSpeed <= Speed)
- {
- NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
- }
- else
- {
- pMD->MotorSetSpeed = Speed;
- pMD->TurnParameter = NewTurnParameter;
- pMD->MotorRampDownIncrement = 0;
- pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount;
- pMD->MotorRampDownCount = 0;
- }
- }
- else
- {
- if (pMD->MotorSetSpeed >= Speed)
- {
- NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
- }
- else
- {
- pMD->MotorSetSpeed = Speed;
- pMD->TurnParameter = NewTurnParameter;
- pMD->MotorRampDownIncrement = 0;
- pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount;
- pMD->MotorRampDownCount = 0;
- }
- }
- }
- break;
- }
- pMD->MotorRunState = NewMotorRunState;
- pMD->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)
-{
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (pMD->MotorTargetSpeed == 0)
- {
- if (pMD->MotorSetSpeed > 0)
- {
- pMD->MotorTargetSpeed = MIN_MOVEMENT_POWER;
- }
- else
- {
- pMD->MotorTargetSpeed = -MIN_MOVEMENT_POWER;
- }
- }
- else
- {
- if (pMD->MotorRampUpIncrement == 0)
- {
- SWORD delta = (SWORD)((pMD->MotorTachoCountToRun - pMD->MotorRampTachoCountStart) / (pMD->MotorSetSpeed - pMD->MotorTargetSpeed));
- if (pMD->MotorSetSpeed > 0)
- {
- pMD->MotorRampUpIncrement = delta;
- }
- else
- {
- pMD->MotorRampUpIncrement = -delta;
- }
- pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount;
- }
- if (pMD->MotorSetSpeed > 0)
- {
- if (pMD->CurrentCaptureCount > (pMD->MotorRampTachoCountOld + pMD->MotorRampUpIncrement))
- {
- pMD->MotorTargetSpeed += 1;
- pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount;
- pMD->MotorRampUpCount = 0;
- }
- else
- {
- if (!(pMD->RegulationMode & REGSTATE_REGULATED))
- {
- pMD->MotorRampUpCount++;
- if (pMD->MotorRampUpCount > 100)
- {
- pMD->MotorRampUpCount = 0;
- pMD->MotorTargetSpeed++;
- }
- }
- }
- }
- else
- {
- if (pMD->CurrentCaptureCount < (pMD->MotorRampTachoCountOld + pMD->MotorRampUpIncrement))
- {
- pMD->MotorTargetSpeed -= 1;
- pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount;
- pMD->MotorRampUpCount = 0;
- }
- else
- {
- if (!(pMD->RegulationMode & REGSTATE_REGULATED))
- {
- pMD->MotorRampUpCount++;
- if (pMD->MotorRampUpCount > 100)
- {
- pMD->MotorRampUpCount = 0;
- pMD->MotorTargetSpeed--;
- }
- }
- }
- }
- }
- if (pMD->MotorSetSpeed > 0)
- {
- if ((pMD->CurrentCaptureCount - pMD->MotorRampTachoCountStart) >= (pMD->MotorTachoCountToRun - pMD->MotorRampTachoCountStart))
- {
- pMD->MotorTargetSpeed = pMD->MotorSetSpeed;
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
- }
- }
- else
- {
- if ((pMD->CurrentCaptureCount + pMD->MotorRampTachoCountStart) <= (pMD->MotorTachoCountToRun + pMD->MotorRampTachoCountStart))
- {
- pMD->MotorTargetSpeed = pMD->MotorSetSpeed;
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
- }
- }
- if (pMD->MotorSetSpeed > 0)
- {
- if (pMD->MotorTargetSpeed > pMD->MotorSetSpeed)
- {
- pMD->MotorTargetSpeed = pMD->MotorSetSpeed;
- }
- }
- else
- {
- if (pMD->MotorTargetSpeed < pMD->MotorSetSpeed)
- {
- pMD->MotorTargetSpeed = pMD->MotorSetSpeed;
- }
- }
- if (pMD->RegulationMode == REGSTATE_IDLE)
- {
- pMD->MotorActualSpeed = pMD->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)
-{
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (pMD->MotorRampDownIncrement == 0)
- {
- if (pMD->MotorTargetSpeed > 0)
- {
- if ((pMD->MotorTargetSpeed > MIN_MOVEMENT_POWER) && (pMD->MotorSetSpeed == 0))
- {
- pMD->MotorRampDownIncrement = ((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / ((pMD->MotorTargetSpeed - pMD->MotorSetSpeed) - MIN_MOVEMENT_POWER));
- }
- else
- {
- pMD->MotorRampDownIncrement = ((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / (pMD->MotorTargetSpeed - pMD->MotorSetSpeed));
- }
- }
- else
- {
- if ((pMD->MotorTargetSpeed < -MIN_MOVEMENT_POWER) && (pMD->MotorSetSpeed == 0))
- {
- pMD->MotorRampDownIncrement = (-((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / ((pMD->MotorTargetSpeed - pMD->MotorSetSpeed) + MIN_MOVEMENT_POWER)));
- }
- else
- {
- pMD->MotorRampDownIncrement = (-((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / (pMD->MotorTargetSpeed - pMD->MotorSetSpeed)));
- }
- }
- pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount;
- }
- if (pMD->MotorTargetSpeed > 0)
- {
- if (pMD->CurrentCaptureCount > (pMD->MotorRampTachoCountOld + (SLONG)pMD->MotorRampDownIncrement))
- {
- pMD->MotorTargetSpeed--;
- if (pMD->MotorTargetSpeed < MIN_MOVEMENT_POWER)
- {
- pMD->MotorTargetSpeed = MIN_MOVEMENT_POWER;
- }
- pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount;
- pMD->MotorRampDownCount = 0;
- dOutputRampDownSynch(MotorNr);
- }
- else
- {
- if (!(pMD->RegulationMode & REGSTATE_REGULATED))
- {
- pMD->MotorRampDownCount++;
- if (pMD->MotorRampDownCount > (UWORD)(30 * pMD->MotorRampDownIncrement))
- {
- pMD->MotorRampDownCount = (UWORD)(20 * pMD->MotorRampDownIncrement);
- pMD->MotorTargetSpeed++;
- }
- }
- }
- }
- else
- {
- if (pMD->CurrentCaptureCount < (pMD->MotorRampTachoCountOld + (SLONG)pMD->MotorRampDownIncrement))
- {
- pMD->MotorTargetSpeed++;
- if (pMD->MotorTargetSpeed > -MIN_MOVEMENT_POWER)
- {
- pMD->MotorTargetSpeed = -MIN_MOVEMENT_POWER;
- }
- pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount;
- pMD->MotorRampDownCount = 0;
- dOutputRampDownSynch(MotorNr);
- }
- else
- {
- if (!(pMD->RegulationMode & REGSTATE_REGULATED))
- {
- pMD->MotorRampDownCount++;
- if (pMD->MotorRampDownCount > (UWORD)(30 * (-pMD->MotorRampDownIncrement)))
- {
- pMD->MotorRampDownCount = (UWORD)(20 * (-pMD->MotorRampDownIncrement));
- pMD->MotorTargetSpeed--;
- }
- }
- }
- }
- if ((pMD->RegulationMode & REGSTATE_SYNCHRONE) && (pMD->TurnParameter != 0))
- {
- dOutputSyncTachoLimitControl(MotorNr);
- if (pMD->MotorRunState == MOTOR_RUN_STATE_IDLE)
- {
- dOutputMotorReachedTachoLimit(MotorNr);
- }
- }
- else
- {
- if (pMD->MotorTargetSpeed > 0)
- {
- if (pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun)
- {
- dOutputMotorReachedTachoLimit(MotorNr);
- }
- }
- else
- {
- if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun)
- {
- dOutputMotorReachedTachoLimit(MotorNr);
- }
- }
- }
- if (pMD->RegulationMode == REGSTATE_IDLE)
- {
- pMD->MotorActualSpeed = pMD->MotorTargetSpeed;
- }
-}
-
-/* Function used to tell whether the wanted position is obtained */
-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)
- {
- dOutputSyncTachoLimitControl(MotorNr);
- }
- else
- {
- if (pMD->MotorSetSpeed > 0)
- {
- if ((pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun))
- {
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pMD->RegulationMode = REGSTATE_IDLE;
- }
- }
- else
- {
- if (pMD->MotorSetSpeed < 0)
- {
- if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun)
- {
- pMD->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pMD->RegulationMode = REGSTATE_IDLE;
- }
- }
- }
- }
- }
- else
- {
- if (pMD->CurrentCaptureCount > MAX_COUNT_TO_RUN)
- {
- pMD->CurrentCaptureCount = 0;
- }
- if (pMD->MotorTargetSpeed != 0)
- {
- pMD->MotorTachoCountToRun = pMD->CurrentCaptureCount;
- }
- }
- if (pMD->RegulationMode == REGSTATE_IDLE)
- {
- pMD->MotorActualSpeed = pMD->MotorTargetSpeed;
- }
-}
-
-/* Function used to decrease speed slowly when the motor is set to idle */
-void dOutputMotorIdleControl(UBYTE MotorNr)
-{
- INSERTMode(MotorNr, COAST_MOTOR_MODE);
-
- MOTORDATA * pMD = &(MotorData[MotorNr]);
-
- if (pMD->MotorActualSpeed != 0)
- {
- if (pMD->MotorActualSpeed > 0)
- {
- pMD->MotorActualSpeed--;
- }
- else
- {
- pMD->MotorActualSpeed++;
- }
- }
-
- if (pMD->MotorTargetSpeed != 0)
- {
- if (pMD->MotorTargetSpeed > 0)
- {
- pMD->MotorTargetSpeed--;
- }
- else
- {
- pMD->MotorTargetSpeed++;
- }
- }
-
- if (pMD->MotorSetSpeed != 0)
- {
- if (pMD->MotorSetSpeed > 0)
- {
- pMD->MotorSetSpeed--;
- }
- else
- {
- pMD->MotorSetSpeed++;
- }
- }
-}
-
-/* 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)
-{
- UBYTE SyncMotorOne;
- UBYTE SyncMotorTwo;
-
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (pMD->RegulationMode & REGSTATE_POSITION)
- {
- dOutputAbsolutePositionRegulation(MotorNr);
- }
- else if (pMD->RegulationMode & REGSTATE_REGULATED)
- {
- dOutputCalculateMotorPosition(MotorNr);
- }
- else
- {
- if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
- {
- dOutputMotorSyncStatus(MotorNr, &SyncMotorOne, &SyncMotorTwo);
-
- if ((SyncMotorOne != 0xFF) &&(SyncMotorTwo != 0xFF))
- {
- dOutputSyncMotorPosition(SyncMotorOne, SyncMotorTwo);
- }
- }
- }
-}
-
-/* Compute PID regulation result for a given error. */
-SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError)
-{
- SLONG PValue, DValue, IValue, TotalRegValue;
-
- MOTORDATA *pMD = &MotorData[MotorNr];
-
- PositionError = dOutputBound (PositionError, 32000);
-
- PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV);
-
- DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV);
- pMD->OldPositionError = PositionError;
-
- pMD->AccError = (pMD->AccError * 3 + PositionError) / 4;
- pMD->AccError = dOutputBound (pMD->AccError, 800);
-
- IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV);
-
- if (!(RegulationOptions & REGOPTION_NO_SATURATION))
- {
- PValue = dOutputBound (PValue, REG_MAX_VALUE);
- IValue = dOutputBound (IValue, REG_MAX_VALUE);
- }
-
- TotalRegValue = (PValue + IValue + DValue) / 2;
-
- if (TotalRegValue > MAXIMUM_SPEED_FW)
- {
- TotalRegValue = MAXIMUM_SPEED_FW;
- pMD->MotorOverloaded = 1;
- }
- 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;
-}
-
-/* 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;
- SLONG PValue;
- SLONG IValue;
- SLONG DValue;
- SLONG CorrectionValue;
- SLONG MotorSpeed;
-
- MOTORDATA * pOne = &(MotorData[MotorOne]);
- MOTORDATA * pTwo = &(MotorData[MotorTwo]);
- SyncData.SyncTachoDif = (SLONG)((pOne->MotorBlockTachoCount) - (pTwo->MotorBlockTachoCount));
-
- if (pOne->TurnParameter != 0)
- {
- if ((pOne->MotorBlockTachoCount != 0) || (pTwo->MotorBlockTachoCount != 0))
- {
- if (pOne->MotorTargetSpeed >= 0)
- {
- if (pOne->TurnParameter > 0)
- {
- TempTurnParameter = (SLONG)(((SLONG)pTwo->TurnParameter * (SLONG)pTwo->MotorTargetSpeed)/100);
- }
- else
- {
- TempTurnParameter = (SLONG)(((SLONG)pOne->TurnParameter * (SLONG)pOne->MotorTargetSpeed)/100);
- }
- }
- else
- {
- if (pOne->TurnParameter > 0)
- {
- TempTurnParameter = (SLONG)(((SLONG)pOne->TurnParameter * (-(SLONG)pOne->MotorTargetSpeed))/100);
- }
- else
- {
- TempTurnParameter = (SLONG)(((SLONG)pTwo->TurnParameter * (-(SLONG)pTwo->MotorTargetSpeed))/100);
- }
- }
- }
- else
- {
- TempTurnParameter = pOne->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;
- SyncData.SyncTachoDif = dOutputBound (SyncData.SyncTachoDif, 500);
-
- PValue = SyncData.SyncTachoDif * (pOne->RegPParameter/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);
-
- IValue = SyncData.SyncAccError * (pOne->RegIParameter/REG_CONST_DIV);
-
- CorrectionValue = (PValue + IValue + DValue) / 4;
-
- MotorSpeed = pOne->MotorTargetSpeed - CorrectionValue;
- MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
-
- if (pOne->TurnParameter != 0)
- {
- if (pOne->MotorTargetSpeed > 0)
- {
- MotorSpeed = dOutputBound (MotorSpeed, pOne->MotorTargetSpeed);
- }
- else
- {
- MotorSpeed = dOutputBound (MotorSpeed, -pOne->MotorTargetSpeed);
- }
- }
- pOne->MotorActualSpeed = (SBYTE)MotorSpeed;
-
- MotorSpeed = pTwo->MotorTargetSpeed + CorrectionValue;
- MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
-
- if (pOne->TurnParameter != 0)
- {
- if (pTwo->MotorTargetSpeed > 0)
- {
- MotorSpeed = dOutputBound (MotorSpeed, pTwo->MotorTargetSpeed);
- }
- else
- {
- MotorSpeed = dOutputBound (MotorSpeed, -pTwo->MotorTargetSpeed);
- }
- }
- pTwo->MotorActualSpeed = (SBYTE)MotorSpeed;
-}
-
-//Called when the motor is ramping down
-void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
-{
- MOTORDATA * pOne = &(MotorData[MotorNr]);
- if (pOne->RegulationMode & REGSTATE_SYNCHRONE)
- {
- UBYTE MotorOne, MotorTwo;
- MotorOne = MotorNr;
- MotorTwo = 0xFF;
- UBYTE i;
- for(i = MOTOR_A; i <= MOTOR_C; i++) {
- if (i == MotorOne)
- continue;
- if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
- MotorTwo = i;
- break;
- }
- }
- pOne->MotorSetSpeed = 0;
- pOne->MotorTargetSpeed = 0;
- pOne->MotorActualSpeed = 0;
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pOne->RegulationMode = REGSTATE_IDLE;
- 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;
- }
- }
- else
- {
- if (pOne->MotorSetSpeed == 0)
- {
- pOne->MotorTargetSpeed = 0;
- pOne->MotorActualSpeed = 0;
- }
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pOne->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;
-
- MotorOne = MotorNr;
- MotorTwo = 0xFF;
- // Synchronisation is done two times, as this function is called for each
- // motor. This is the same behaviour as previous code.
- UBYTE i;
- for(i = MOTOR_A; i <= MOTOR_C; i++) {
- if (i == MotorOne)
- continue;
- if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
- MotorTwo = i;
- break;
- }
- }
- if (MotorTwo == 0xFF)
- MotorOne = 0xFF;
-
- if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
- {
- MOTORDATA * pOne = &(MotorData[MotorOne]);
- MOTORDATA * pTwo = &(MotorData[MotorTwo]);
- if (pOne->TurnParameter != 0)
- {
- if (pOne->TurnParameter > 0)
- {
- if (pTwo->MotorTargetSpeed >= 0)
- {
- if ((SLONG)(pTwo->CurrentCaptureCount >= pTwo->MotorTachoCountToRun))
- {
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
-
- pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount;
- pOne->MotorTachoCountToRun = pTwo->MotorTachoCountToRun;
- }
- }
- else
- {
- if ((SLONG)(pOne->CurrentCaptureCount <= pOne->MotorTachoCountToRun))
- {
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
-
- pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount;
- pTwo->MotorTachoCountToRun = pOne->MotorTachoCountToRun;
- }
- }
- }
- else
- {
- if (pOne->MotorTargetSpeed >= 0)
- {
- if ((SLONG)(pOne->CurrentCaptureCount >= pOne->MotorTachoCountToRun))
- {
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
-
- pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount;
- pTwo->MotorTachoCountToRun = pOne->MotorTachoCountToRun;
- }
- }
- else
- {
- if ((SLONG)(pTwo->CurrentCaptureCount <= pTwo->MotorTachoCountToRun))
- {
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
-
- pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount;
- pOne->MotorTachoCountToRun = pTwo->MotorTachoCountToRun;
- }
- }
- }
- }
- else
- {
- if (pOne->MotorSetSpeed > 0)
- {
- if ((pOne->CurrentCaptureCount >= pOne->MotorTachoCountToRun) || (pTwo->CurrentCaptureCount >= pTwo->MotorTachoCountToRun))
- {
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE;
- }
- }
- else
- {
- if (pOne->MotorSetSpeed < 0)
- {
- if ((pOne->CurrentCaptureCount <= pOne->MotorTachoCountToRun) || (pTwo->CurrentCaptureCount <= pTwo->MotorTachoCountToRun))
- {
- pOne->MotorRunState = MOTOR_RUN_STATE_IDLE;
- pTwo->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;
-
- MotorOne = MotorNr;
- MotorTwo = 0xFF;
- UBYTE i;
- for(i = MOTOR_A; i <= MOTOR_C; i++) {
- if (i == MotorOne)
- continue;
- if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
- MotorTwo = i;
- break;
- }
- }
- 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->MotorTachoCountTarget = 0;
- pTwo->CurrentCaptureCount = 0;
- pTwo->MotorTachoCountToRun = 0;
- pTwo->MotorTachoCountTarget = 0;
- }
- else
- {
- pMD->CurrentCaptureCount = 0;
- pMD->MotorTachoCountToRun = 0;
- pMD->MotorTachoCountTarget = 0;
- }
-}
-
-/* Function which is called when motors are synchronized and motor is ramping down */
-void dOutputRampDownSynch(UBYTE MotorNr)
-{
- UBYTE MotorOne, MotorTwo;
-
- MotorOne = MotorNr;
- MotorTwo = 0xFF;
- UBYTE i;
- for(i = MOTOR_A; i <= MOTOR_C; i++) {
- if (i == MotorOne)
- continue;
- if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
- MotorTwo = i;
- break;
- }
- }
- if (MotorTwo == 0xFF)
- MotorOne = 0xFF;
-
- if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
- {
- MOTORDATA * pOne = &(MotorData[MotorOne]);
- MOTORDATA * pTwo = &(MotorData[MotorTwo]);
- if (pOne->TurnParameter != 0)
- {
- if (pOne->TurnParameter > 0)
- {
- if (pOne->MotorTargetSpeed >= 0)
- {
- if (pTwo->MotorActualSpeed < 0)
- {
- pTwo->MotorTargetSpeed--;
- }
- }
- else
- {
- if (pTwo->MotorActualSpeed > 0)
- {
- pTwo->MotorTargetSpeed++;
- }
- }
- }
- else
- {
- if (pOne->MotorTargetSpeed >= 0)
- {
- if (pTwo->MotorActualSpeed < 0)
- {
- pTwo->MotorTargetSpeed--;
- }
- }
- else
- {
- if (pTwo->MotorActualSpeed > 0)
- {
- pTwo->MotorTargetSpeed++;
- }
- }
- }
- }
- }
-}
-