From d383f2bcdb6ff13cc562fce1ff55d826035debad Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 6 Mar 2023 17:36:31 +0100 Subject: Simplify source tree Now just use make in the root directory to build. --- AT91SAM7S256/Source/d_output.c | 1415 ---------------------------------------- 1 file changed, 1415 deletions(-) delete mode 100644 AT91SAM7S256/Source/d_output.c (limited to 'AT91SAM7S256/Source/d_output.c') 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 - -#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++; - } - } - } - } - } -} - -- cgit v1.2.3