From 6c5075bc99ba07f46562f7f10db14e153d7a93ec Mon Sep 17 00:00:00 2001 From: jhansen Date: Mon, 26 Apr 2010 21:49:44 +0000 Subject: git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc_128/trunk@6 c9361245-7fe8-9947-84e8-057757c4e366 --- AT91SAM7S256/Source/d_output.c | 897 ++++++++++++++++++++++++----------------- 1 file changed, 516 insertions(+), 381 deletions(-) (limited to 'AT91SAM7S256/Source/d_output.c') diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index d953b84..64f5e56 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -27,14 +27,14 @@ #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 +#define OPTION_HOLDATLIMIT 0x10 +#define OPTION_RAMPDOWNTOLIMIT 0x20 + void dOutputRampDownSynch(UBYTE MotorNr); typedef struct @@ -64,6 +64,10 @@ typedef struct SLONG MotorRampTachoCountOld; // Used to hold old position during Ramp-Up SLONG MotorRampTachoCountStart; // Used to hold position when Ramp-up started SLONG RotationCaptureCount; // Counter for additional rotation counter + UBYTE RunStateAtLimit; // what run state to switch to when tacho limit is reached + UBYTE RampDownToLimit; + UBYTE Spare2; + UBYTE Spare3; }MOTORDATA; typedef struct @@ -77,6 +81,8 @@ typedef struct static MOTORDATA MotorData[3]; static SYNCMOTORDATA SyncData; +static UBYTE RegTime; + void dOutputInit(void) { UBYTE Temp; @@ -85,30 +91,35 @@ void dOutputInit(void) ENABLECaptureMotorA; ENABLECaptureMotorB; ENABLECaptureMotorC; + + RegTime = REGULATION_TIME; 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; + 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->RegulationTimeCount = 0; + pMD->RegPParameter = DEFAULT_P_GAIN_FACTOR; + pMD->RegIParameter = DEFAULT_I_GAIN_FACTOR; + pMD->RegDParameter = DEFAULT_D_GAIN_FACTOR; + pMD->RegulationMode = 0; + pMD->MotorOverloaded = 0; + pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE; + pMD->RampDownToLimit = 0; INSERTMode(Temp, COAST_MOTOR_MODE); - INSERTSpeed(Temp, MotorData[Temp].MotorSetSpeed); + INSERTSpeed(Temp, pMD->MotorSetSpeed); } } -/* This function is called every 1 mS and will go through all the motors and there dependencies */ +/* This function is called every 1 mS and will go through all the motors and their dependencies */ /* Actual motor speed is only passed (updated) to the AVR controller form this function */ /* DeltacaptureCount used to count number of Tachocount within last 100 mS. Used with position control regulation */ /* CurrentCaptureCount used to tell total current position. Used to tell when movement has been obtained */ @@ -123,43 +134,43 @@ void dOutputCtrl(void) 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++; + MOTORDATA * pMD = &(MotorData[MotorNr]); + pMD->DeltaCaptureCount += NewTachoCount[MotorNr]; + pMD->CurrentCaptureCount += NewTachoCount[MotorNr]; + pMD->MotorBlockTachoCount += NewTachoCount[MotorNr]; + pMD->RotationCaptureCount += NewTachoCount[MotorNr]; + pMD->RegulationTimeCount++; - if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RAMPUP) + if (pMD->MotorRunState == MOTOR_RUN_STATE_RAMPUP) { dOutputRampUpFunction(MotorNr); } - if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RAMPDOWN) + if (pMD->MotorRunState == MOTOR_RUN_STATE_RAMPDOWN) { dOutputRampDownFunction(MotorNr); } - if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RUNNING) + if (pMD->MotorRunState == MOTOR_RUN_STATE_RUNNING) { dOutputTachoLimitControl(MotorNr); } - if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_IDLE) + if (pMD->MotorRunState == MOTOR_RUN_STATE_IDLE) { dOutputMotorIdleControl(MotorNr); } - if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_HOLD) + if (pMD->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; - + pMD->MotorSetSpeed = 0; + pMD->MotorActualSpeed = 0; + pMD->MotorTargetSpeed = 0; + pMD->RegulationTimeCount = 0; + pMD->DeltaCaptureCount = 0; +// pMD->MotorRunState = MOTOR_RUN_STATE_RUNNING; } - if (MotorData[MotorNr].RegulationTimeCount > REGULATION_TIME) + if (pMD->RegulationTimeCount > RegTime) { - MotorData[MotorNr].RegulationTimeCount = 0; + pMD->RegulationTimeCount = 0; dOutputRegulateMotor(MotorNr); - MotorData[MotorNr].DeltaCaptureCount = 0; + pMD->DeltaCaptureCount = 0; } } INSERTSpeed(MOTOR_A, MotorData[MOTOR_A].MotorActualSpeed); @@ -180,12 +191,13 @@ void dOutputGetMotorParameters(UBYTE *CurrentMotorSpeed, SLONG *TachoCount, SLON 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; + 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; } } @@ -199,17 +211,18 @@ void dOutputSetMode(UBYTE Motor, UBYTE Mode) //Set motor mode (break, Float) /* AccError & OldPositionError used for position regulation and Sync Parameter are used for synchronization regulation */ void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode) { - MotorData[MotorNr].RegulationMode = RegulationMode; + MOTORDATA * pMD = &(MotorData[MotorNr]); + pMD->RegulationMode = RegulationMode; - if ((MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED) && (MotorData[MotorNr].MotorSetSpeed == 0) && (MotorData[MotorNr].MotorRunState != MOTOR_RUN_STATE_RAMPDOWN)) + if ((pMD->RegulationMode & REGSTATE_REGULATED) && (pMD->MotorSetSpeed == 0) && (pMD->MotorRunState != MOTOR_RUN_STATE_RAMPDOWN)) { - MotorData[MotorNr].AccError = 0; - MotorData[MotorNr].OldPositionError = 0; + pMD->AccError = 0; + pMD->OldPositionError = 0; } - if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) + if (pMD->RegulationMode & REGSTATE_SYNCHRONE) { - if (((MotorData[MotorNr].MotorActualSpeed == 0) || (MotorData[MotorNr].TurnParameter != 0) || (MotorData[MotorNr].TurnParameter == 0)) && (MotorData[MotorNr].MotorRunState != MOTOR_RUN_STATE_RAMPDOWN)) + if (((pMD->MotorActualSpeed == 0) || (pMD->TurnParameter != 0) || (pMD->TurnParameter == 0)) && (pMD->MotorRunState != MOTOR_RUN_STATE_RAMPDOWN)) { SyncData.SyncTachoDif = 0; @@ -230,17 +243,18 @@ void dOutputDisableRegulation(UBYTE MotorNr) /* 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; + MOTORDATA * pMD = &(MotorData[MotorNr]); + pMD->CurrentCaptureCount = 0; + pMD->MotorTachoCountToRun = 0; - if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) + if (pMD->RegulationMode & REGSTATE_SYNCHRONE) { dOutputResetSyncMotors(MotorNr); } - if (MotorData[MotorNr].MotorRunForever == 1) + if (pMD->MotorRunForever == 1) { - MotorData[MotorNr].MotorRunForever = 0; // To ensure that we get the same functionality for all combination on motor durations + pMD->MotorRunForever = 0; // To ensure that we get the same functionality for all combination on motor durations } } @@ -259,46 +273,52 @@ void dOutputResetRotationCaptureCount(UBYTE MotorNr) } /* Can be used to set new PID values */ -void dOutputSetPIDParameters(UBYTE Motor, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter) +void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter) { - MotorData[Motor].RegPParameter = NewRegPParameter; - MotorData[Motor].RegIParameter = NewRegIParameter; - MotorData[Motor].RegDParameter = NewRegDParameter; + MOTORDATA * pMD = &(MotorData[MotorNr]); + pMD->RegPParameter = NewRegPParameter; + pMD->RegIParameter = NewRegIParameter; + pMD->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) +void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel, UBYTE Options) { + MOTORDATA * pMD = &(MotorData[MotorNr]); if (BlockTachoCntToTravel == 0) { - MotorData[MotorNr].MotorRunForever = 1; + pMD->MotorRunForever = 1; + pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE; + pMD->RampDownToLimit = 0; } else { - MotorData[MotorNr].MotorRunForever = 0; + pMD->MotorRunForever = 0; + pMD->RunStateAtLimit = (Options & OPTION_HOLDATLIMIT) ? MOTOR_RUN_STATE_HOLD : MOTOR_RUN_STATE_IDLE; + pMD->RampDownToLimit = Options & OPTION_RAMPDOWNTOLIMIT; - if (MotorData[MotorNr].MotorSetSpeed == 0) + if (pMD->MotorSetSpeed == 0) { - if (MotorData[MotorNr].MotorTargetSpeed > 0) + if (pMD->MotorTargetSpeed > 0) { - MotorData[MotorNr].MotorTachoCountToRun += BlockTachoCntToTravel; + pMD->MotorTachoCountToRun += BlockTachoCntToTravel; } else { - MotorData[MotorNr].MotorTachoCountToRun -= BlockTachoCntToTravel; + pMD->MotorTachoCountToRun -= BlockTachoCntToTravel; } } else { - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->MotorSetSpeed > 0) { - MotorData[MotorNr].MotorTachoCountToRun += BlockTachoCntToTravel; + pMD->MotorTachoCountToRun += BlockTachoCntToTravel; } else { - MotorData[MotorNr].MotorTachoCountToRun -= BlockTachoCntToTravel; + pMD->MotorTachoCountToRun -= BlockTachoCntToTravel; } } } @@ -307,67 +327,69 @@ void dOutputSetTachoLimit(UBYTE MotorNr, ULONG 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)) + MOTORDATA * pMD = &(MotorData[MotorNr]); + if ((pMD->MotorSetSpeed != Speed) || (pMD->MotorRunState != NewMotorRunState) || + (NewMotorRunState == MOTOR_RUN_STATE_IDLE) || (pMD->TurnParameter != NewTurnParameter)) { - if (MotorData[MotorNr].MotorTargetSpeed == 0) + if (pMD->MotorTargetSpeed == 0) { - MotorData[MotorNr].AccError = 0; - MotorData[MotorNr].OldPositionError = 0; - MotorData[MotorNr].RegulationTimeCount = 0; - MotorData[MotorNr].DeltaCaptureCount = 0; + pMD->AccError = 0; + pMD->OldPositionError = 0; + pMD->RegulationTimeCount = 0; + pMD->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; + //pMD->MotorSetSpeed = 0; + //pMD->MotorTargetSpeed = 0; + //pMD->TurnParameter = 0; + pMD->RegulationMode = REGSTATE_IDLE; } break; case MOTOR_RUN_STATE_RAMPUP: { - if (MotorData[MotorNr].MotorSetSpeed == 0) + if (pMD->MotorSetSpeed == 0) { - MotorData[MotorNr].MotorSetSpeed = Speed; - MotorData[MotorNr].TurnParameter = NewTurnParameter; - MotorData[MotorNr].MotorRampUpIncrement = 0; - MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount; - MotorData[MotorNr].MotorRampUpCount = 0; + pMD->MotorSetSpeed = Speed; + pMD->TurnParameter = NewTurnParameter; + pMD->MotorRampUpIncrement = 0; + pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount; + pMD->MotorRampUpCount = 0; } else { if (Speed > 0) { - if (MotorData[MotorNr].MotorSetSpeed >= Speed) + if (pMD->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; + pMD->MotorSetSpeed = Speed; + pMD->TurnParameter = NewTurnParameter; + pMD->MotorRampUpIncrement = 0; + pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount; + pMD->MotorRampUpCount = 0; } } else { - if (MotorData[MotorNr].MotorSetSpeed <= Speed) + if (pMD->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; + pMD->MotorSetSpeed = Speed; + pMD->TurnParameter = NewTurnParameter; + pMD->MotorRampUpIncrement = 0; + pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount; + pMD->MotorRampUpCount = 0; } } } @@ -376,11 +398,11 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE case MOTOR_RUN_STATE_RUNNING: { - MotorData[MotorNr].MotorSetSpeed = Speed; - MotorData[MotorNr].MotorTargetSpeed = Speed; - MotorData[MotorNr].TurnParameter = NewTurnParameter; + pMD->MotorSetSpeed = Speed; + pMD->MotorTargetSpeed = Speed; + pMD->TurnParameter = NewTurnParameter; - if (MotorData[MotorNr].MotorSetSpeed == 0) + if (pMD->MotorSetSpeed == 0) { NewMotorRunState = MOTOR_RUN_STATE_HOLD; } @@ -389,41 +411,41 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE case MOTOR_RUN_STATE_RAMPDOWN: { - if (MotorData[MotorNr].MotorTargetSpeed >= 0) + if (pMD->MotorTargetSpeed >= 0) { - if (MotorData[MotorNr].MotorSetSpeed <= Speed) + if (pMD->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; + pMD->MotorSetSpeed = Speed; + pMD->TurnParameter = NewTurnParameter; + pMD->MotorRampDownIncrement = 0; + pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount; + pMD->MotorRampDownCount = 0; } } else { - if (MotorData[MotorNr].MotorSetSpeed >= Speed) + if (pMD->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; + pMD->MotorSetSpeed = Speed; + pMD->TurnParameter = NewTurnParameter; + pMD->MotorRampDownIncrement = 0; + pMD->MotorRampTachoCountStart = pMD->CurrentCaptureCount; + pMD->MotorRampDownCount = 0; } } } break; } - MotorData[MotorNr].MotorRunState = NewMotorRunState; - MotorData[MotorNr].MotorOverloaded = 0; + pMD->MotorRunState = NewMotorRunState; + pMD->MotorOverloaded = 0; } } @@ -431,107 +453,109 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE /* 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) + MOTORDATA * pMD = &(MotorData[MotorNr]); + if (pMD->MotorTargetSpeed == 0) { - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->MotorSetSpeed > 0) { - MotorData[MotorNr].MotorTargetSpeed = MIN_MOVEMENT_POWER; + pMD->MotorTargetSpeed = MIN_MOVEMENT_POWER; } else { - MotorData[MotorNr].MotorTargetSpeed = -MIN_MOVEMENT_POWER; + pMD->MotorTargetSpeed = -MIN_MOVEMENT_POWER; } } else { - if (MotorData[MotorNr].MotorRampUpIncrement == 0) + if (pMD->MotorRampUpIncrement == 0) { - if (MotorData[MotorNr].MotorSetSpeed > 0) + SWORD delta = (SWORD)((pMD->MotorTachoCountToRun - pMD->MotorRampTachoCountStart) / (pMD->MotorSetSpeed - pMD->MotorTargetSpeed)); + if (pMD->MotorSetSpeed > 0) { - MotorData[MotorNr].MotorRampUpIncrement = (SWORD)((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart) / (MotorData[MotorNr].MotorSetSpeed - MotorData[MotorNr].MotorTargetSpeed)); + pMD->MotorRampUpIncrement = delta; } else { - MotorData[MotorNr].MotorRampUpIncrement = (SWORD)(-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart) / (MotorData[MotorNr].MotorSetSpeed - MotorData[MotorNr].MotorTargetSpeed))); + pMD->MotorRampUpIncrement = -delta; } - MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount; + pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount; } - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->MotorSetSpeed > 0) { - if (MotorData[MotorNr].CurrentCaptureCount > (MotorData[MotorNr].MotorRampTachoCountOld + MotorData[MotorNr].MotorRampUpIncrement)) + if (pMD->CurrentCaptureCount > (pMD->MotorRampTachoCountOld + pMD->MotorRampUpIncrement)) { - MotorData[MotorNr].MotorTargetSpeed += 1; - MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount; - MotorData[MotorNr].MotorRampUpCount = 0; + pMD->MotorTargetSpeed++; + pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount; + pMD->MotorRampUpCount = 0; } else { - if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)) + if (!(pMD->RegulationMode & REGSTATE_REGULATED)) { - MotorData[MotorNr].MotorRampUpCount++; - if (MotorData[MotorNr].MotorRampUpCount > 100) + pMD->MotorRampUpCount++; + if (pMD->MotorRampUpCount > RegTime) { - MotorData[MotorNr].MotorRampUpCount = 0; - MotorData[MotorNr].MotorTargetSpeed++; + pMD->MotorRampUpCount = 0; + pMD->MotorTargetSpeed++; } } } } else { - if (MotorData[MotorNr].CurrentCaptureCount < (MotorData[MotorNr].MotorRampTachoCountOld + MotorData[MotorNr].MotorRampUpIncrement)) + if (pMD->CurrentCaptureCount < (pMD->MotorRampTachoCountOld + pMD->MotorRampUpIncrement)) { - MotorData[MotorNr].MotorTargetSpeed -= 1; - MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount; - MotorData[MotorNr].MotorRampUpCount = 0; + pMD->MotorTargetSpeed--; + pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount; + pMD->MotorRampUpCount = 0; } else { - if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)) + if (!(pMD->RegulationMode & REGSTATE_REGULATED)) { - MotorData[MotorNr].MotorRampUpCount++; - if (MotorData[MotorNr].MotorRampUpCount > 100) + pMD->MotorRampUpCount++; + if (pMD->MotorRampUpCount > RegTime) { - MotorData[MotorNr].MotorRampUpCount = 0; - MotorData[MotorNr].MotorTargetSpeed--; + pMD->MotorRampUpCount = 0; + pMD->MotorTargetSpeed--; } } } } } - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->MotorSetSpeed > 0) { - if ((MotorData[MotorNr].CurrentCaptureCount - MotorData[MotorNr].MotorRampTachoCountStart) >= (MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart)) + if ((pMD->CurrentCaptureCount - pMD->MotorRampTachoCountStart) >= (pMD->MotorTachoCountToRun - pMD->MotorRampTachoCountStart)) { - MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; + pMD->MotorTargetSpeed = pMD->MotorSetSpeed; + pMD->MotorRunState = pMD->RunStateAtLimit; } } else { - if ((MotorData[MotorNr].CurrentCaptureCount + MotorData[MotorNr].MotorRampTachoCountStart) <= (MotorData[MotorNr].MotorTachoCountToRun + MotorData[MotorNr].MotorRampTachoCountStart)) + if ((pMD->CurrentCaptureCount + pMD->MotorRampTachoCountStart) <= (pMD->MotorTachoCountToRun + pMD->MotorRampTachoCountStart)) { - MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; + pMD->MotorTargetSpeed = pMD->MotorSetSpeed; + pMD->MotorRunState = pMD->RunStateAtLimit; } } - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->MotorSetSpeed > 0) { - if (MotorData[MotorNr].MotorTargetSpeed > MotorData[MotorNr].MotorSetSpeed) + if (pMD->MotorTargetSpeed > pMD->MotorSetSpeed) { - MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed; + pMD->MotorTargetSpeed = pMD->MotorSetSpeed; } } else { - if (MotorData[MotorNr].MotorTargetSpeed < MotorData[MotorNr].MotorSetSpeed) + if (pMD->MotorTargetSpeed < pMD->MotorSetSpeed) { - MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed; + pMD->MotorTargetSpeed = pMD->MotorSetSpeed; } } - if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE) + if (pMD->RegulationMode == REGSTATE_IDLE) { - MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed; + pMD->MotorActualSpeed = pMD->MotorTargetSpeed; } } @@ -539,142 +563,169 @@ void dOutputRampUpFunction(UBYTE MotorNr) /* 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) + MOTORDATA * pMD = &(MotorData[MotorNr]); + if (pMD->MotorRampDownIncrement == 0) { - if (MotorData[MotorNr].MotorTargetSpeed > 0) + if (pMD->MotorTargetSpeed > 0) { - if ((MotorData[MotorNr].MotorTargetSpeed > MIN_MOVEMENT_POWER) && (MotorData[MotorNr].MotorSetSpeed == 0)) + if ((pMD->MotorTargetSpeed > MIN_MOVEMENT_POWER) && (pMD->MotorSetSpeed == 0)) { - MotorData[MotorNr].MotorRampDownIncrement = ((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / ((MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed) - MIN_MOVEMENT_POWER)); + pMD->MotorRampDownIncrement = ((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / ((pMD->MotorTargetSpeed - pMD->MotorSetSpeed) - MIN_MOVEMENT_POWER)); } else { - MotorData[MotorNr].MotorRampDownIncrement = ((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / (MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed)); + pMD->MotorRampDownIncrement = ((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / (pMD->MotorTargetSpeed - pMD->MotorSetSpeed)); } } else { - if ((MotorData[MotorNr].MotorTargetSpeed < -MIN_MOVEMENT_POWER) && (MotorData[MotorNr].MotorSetSpeed == 0)) + if ((pMD->MotorTargetSpeed < -MIN_MOVEMENT_POWER) && (pMD->MotorSetSpeed == 0)) { - MotorData[MotorNr].MotorRampDownIncrement = (-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / ((MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed) + MIN_MOVEMENT_POWER))); + pMD->MotorRampDownIncrement = (-((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / ((pMD->MotorTargetSpeed - pMD->MotorSetSpeed) + MIN_MOVEMENT_POWER))); } else { - MotorData[MotorNr].MotorRampDownIncrement = (-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / (MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed))); + pMD->MotorRampDownIncrement = (-((pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount) / (pMD->MotorTargetSpeed - pMD->MotorSetSpeed))); } } - MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount; + pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount; } - if (MotorData[MotorNr].MotorTargetSpeed > 0) + if (pMD->MotorTargetSpeed > 0) { - if (MotorData[MotorNr].CurrentCaptureCount > (MotorData[MotorNr].MotorRampTachoCountOld + (SLONG)MotorData[MotorNr].MotorRampDownIncrement)) + if (pMD->CurrentCaptureCount > (pMD->MotorRampTachoCountOld + (SLONG)pMD->MotorRampDownIncrement)) { - MotorData[MotorNr].MotorTargetSpeed--; - if (MotorData[MotorNr].MotorTargetSpeed < MIN_MOVEMENT_POWER) + pMD->MotorTargetSpeed--; + if (pMD->MotorTargetSpeed < MIN_MOVEMENT_POWER) { - MotorData[MotorNr].MotorTargetSpeed = MIN_MOVEMENT_POWER; + pMD->MotorTargetSpeed = MIN_MOVEMENT_POWER; } - MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount; - MotorData[MotorNr].MotorRampDownCount = 0; + pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount; + pMD->MotorRampDownCount = 0; dOutputRampDownSynch(MotorNr); } else { - if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)) + if (!(pMD->RegulationMode & REGSTATE_REGULATED)) { - MotorData[MotorNr].MotorRampDownCount++; - if (MotorData[MotorNr].MotorRampDownCount > (UWORD)(30 * MotorData[MotorNr].MotorRampDownIncrement)) + pMD->MotorRampDownCount++; + if (pMD->MotorRampDownCount > (UWORD)(30 * pMD->MotorRampDownIncrement)) { - MotorData[MotorNr].MotorRampDownCount = (UWORD)(20 * MotorData[MotorNr].MotorRampDownIncrement); - MotorData[MotorNr].MotorTargetSpeed++; + pMD->MotorRampDownCount = (UWORD)(20 * pMD->MotorRampDownIncrement); + pMD->MotorTargetSpeed++; } } } } else { - if (MotorData[MotorNr].CurrentCaptureCount < (MotorData[MotorNr].MotorRampTachoCountOld + (SLONG)MotorData[MotorNr].MotorRampDownIncrement)) + if (pMD->CurrentCaptureCount < (pMD->MotorRampTachoCountOld + (SLONG)pMD->MotorRampDownIncrement)) { - MotorData[MotorNr].MotorTargetSpeed++; - if (MotorData[MotorNr].MotorTargetSpeed > -MIN_MOVEMENT_POWER) + pMD->MotorTargetSpeed++; + if (pMD->MotorTargetSpeed > -MIN_MOVEMENT_POWER) { - MotorData[MotorNr].MotorTargetSpeed = -MIN_MOVEMENT_POWER; + pMD->MotorTargetSpeed = -MIN_MOVEMENT_POWER; } - MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount; - MotorData[MotorNr].MotorRampDownCount = 0; + pMD->MotorRampTachoCountOld = pMD->CurrentCaptureCount; + pMD->MotorRampDownCount = 0; dOutputRampDownSynch(MotorNr); } else { - if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)) + if (!(pMD->RegulationMode & REGSTATE_REGULATED)) { - MotorData[MotorNr].MotorRampDownCount++; - if (MotorData[MotorNr].MotorRampDownCount > (UWORD)(30 * (-MotorData[MotorNr].MotorRampDownIncrement))) + pMD->MotorRampDownCount++; + if (pMD->MotorRampDownCount > (UWORD)(30 * (-pMD->MotorRampDownIncrement))) { - MotorData[MotorNr].MotorRampDownCount = (UWORD)(20 * (-MotorData[MotorNr].MotorRampDownIncrement)); - MotorData[MotorNr].MotorTargetSpeed--; + pMD->MotorRampDownCount = (UWORD)(20 * (-pMD->MotorRampDownIncrement)); + pMD->MotorTargetSpeed--; } } } } - if ((MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) && (MotorData[MotorNr].TurnParameter != 0)) + if ((pMD->RegulationMode & REGSTATE_SYNCHRONE) && (pMD->TurnParameter != 0)) { dOutputSyncTachoLimitControl(MotorNr); - if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_IDLE) + if (pMD->MotorRunState == pMD->RunStateAtLimit) { dOutputMotorReachedTachoLimit(MotorNr); } } else { - if (MotorData[MotorNr].MotorTargetSpeed > 0) + if (pMD->MotorTargetSpeed > 0) { - if (MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun) + if (pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun) { dOutputMotorReachedTachoLimit(MotorNr); } } else { - if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun) + if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun) { dOutputMotorReachedTachoLimit(MotorNr); } } } - if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE) + if (pMD->RegulationMode == REGSTATE_IDLE) { - MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed; + pMD->MotorActualSpeed = pMD->MotorTargetSpeed; } } /* Function used to tell whether the wanted position is obtained */ void dOutputTachoLimitControl(UBYTE MotorNr) { - if (MotorData[MotorNr].MotorRunForever == 0) + MOTORDATA * pMD = &(MotorData[MotorNr]); + if (pMD->MotorRunForever == 0) { - if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) + if (pMD->RegulationMode & REGSTATE_SYNCHRONE) { dOutputSyncTachoLimitControl(MotorNr); } else { - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->RampDownToLimit == 0) { - if ((MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun)) + if (pMD->MotorSetSpeed > 0) { - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; + if ((pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun)) + { + pMD->MotorRunState = pMD->RunStateAtLimit; + pMD->RegulationMode = REGSTATE_IDLE; + } + } + else + { + if (pMD->MotorSetSpeed < 0) + { + if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun) + { + pMD->MotorRunState = pMD->RunStateAtLimit; + pMD->RegulationMode = REGSTATE_IDLE; + } + } } } else { - if (MotorData[MotorNr].MotorSetSpeed < 0) + if (pMD->MotorSetSpeed > 0) { - if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun) + if ((pMD->CurrentCaptureCount >= (SLONG)((float)pMD->MotorTachoCountToRun * (float)0.80))) { - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; + pMD->MotorRunState = MOTOR_RUN_STATE_RAMPDOWN; + pMD->MotorSetSpeed = 0; + } + } + else + { + if (pMD->MotorSetSpeed < 0) + { + if (pMD->CurrentCaptureCount <= (SLONG)((float)pMD->MotorTachoCountToRun * (float)0.80)) + { + pMD->MotorRunState = MOTOR_RUN_STATE_RAMPDOWN; + pMD->MotorSetSpeed = 0; + } } } } @@ -682,18 +733,18 @@ void dOutputTachoLimitControl(UBYTE MotorNr) } else { - if (MotorData[MotorNr].CurrentCaptureCount > MAX_COUNT_TO_RUN) + if (pMD->CurrentCaptureCount > MAX_COUNT_TO_RUN) { - MotorData[MotorNr].CurrentCaptureCount = 0; + pMD->CurrentCaptureCount = 0; } - if (MotorData[MotorNr].MotorTargetSpeed != 0) + if (pMD->MotorTargetSpeed != 0) { - MotorData[MotorNr].MotorTachoCountToRun = MotorData[MotorNr].CurrentCaptureCount; + pMD->MotorTachoCountToRun = pMD->CurrentCaptureCount; } } - if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE) + if (pMD->RegulationMode == REGSTATE_IDLE) { - MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed; + pMD->MotorActualSpeed = pMD->MotorTargetSpeed; } } @@ -702,39 +753,41 @@ void dOutputMotorIdleControl(UBYTE MotorNr) { INSERTMode(MotorNr, COAST_MOTOR_MODE); - if (MotorData[MotorNr].MotorActualSpeed != 0) + MOTORDATA * pMD = &(MotorData[MotorNr]); + + if (pMD->MotorActualSpeed != 0) { - if (MotorData[MotorNr].MotorActualSpeed > 0) + if (pMD->MotorActualSpeed > 0) { - MotorData[MotorNr].MotorActualSpeed--; + pMD->MotorActualSpeed--; } else { - MotorData[MotorNr].MotorActualSpeed++; + pMD->MotorActualSpeed++; } } - if (MotorData[MotorNr].MotorTargetSpeed != 0) + if (pMD->MotorTargetSpeed != 0) { - if (MotorData[MotorNr].MotorTargetSpeed > 0) + if (pMD->MotorTargetSpeed > 0) { - MotorData[MotorNr].MotorTargetSpeed--; + pMD->MotorTargetSpeed--; } else { - MotorData[MotorNr].MotorTargetSpeed++; + pMD->MotorTargetSpeed++; } } - if (MotorData[MotorNr].MotorSetSpeed != 0) + if (pMD->MotorSetSpeed != 0) { - if (MotorData[MotorNr].MotorSetSpeed > 0) + if (pMD->MotorSetSpeed > 0) { - MotorData[MotorNr].MotorSetSpeed--; + pMD->MotorSetSpeed--; } else { - MotorData[MotorNr].MotorSetSpeed++; + pMD->MotorSetSpeed++; } } } @@ -745,13 +798,14 @@ void dOutputRegulateMotor(UBYTE MotorNr) UBYTE SyncMotorOne; UBYTE SyncMotorTwo; - if (MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED) + MOTORDATA * pMD = &(MotorData[MotorNr]); + if (pMD->RegulationMode & REGSTATE_REGULATED) { dOutputCalculateMotorPosition(MotorNr); } else { - if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) + if (pMD->RegulationMode & REGSTATE_SYNCHRONE) { dOutputMotorSyncStatus(MotorNr, &SyncMotorOne, &SyncMotorTwo); @@ -774,20 +828,21 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) SWORD TotalRegValue; SWORD NewSpeedCount = 0; - NewSpeedCount = (SWORD)((MotorData[MotorNr].MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR); + MOTORDATA * pMD = &(MotorData[MotorNr]); + NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR); - PositionError = (SWORD)(MotorData[MotorNr].OldPositionError - MotorData[MotorNr].DeltaCaptureCount) + NewSpeedCount; + PositionError = (SWORD)(pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount; //Overflow control on PositionError - if (MotorData[MotorNr].RegPParameter != 0) + if (pMD->RegPParameter != 0) { - if (PositionError > (SWORD)(32000 / MotorData[MotorNr].RegPParameter)) + if (PositionError > (SWORD)(32000 / pMD->RegPParameter)) { - PositionError = (SWORD)(32000 / MotorData[MotorNr].RegPParameter); + PositionError = (SWORD)(32000 / pMD->RegPParameter); } - if (PositionError < (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter))) + if (PositionError < (SWORD)(-(32000 / pMD->RegPParameter))) { - PositionError = (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter)); + PositionError = (SWORD)(-(32000 / pMD->RegPParameter)); } } else @@ -802,7 +857,7 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) } } - PValue = PositionError * (SWORD)(MotorData[MotorNr].RegPParameter/REG_CONST_DIV); + PValue = PositionError * (SWORD)(pMD->RegPParameter/REG_CONST_DIV); if (PValue > (SWORD)REG_MAX_VALUE) { PValue = REG_MAX_VALUE; @@ -812,21 +867,21 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) PValue = REG_MIN_VALUE; } - DValue = (PositionError - MotorData[MotorNr].OldPositionError) * (SWORD)(MotorData[MotorNr].RegDParameter/REG_CONST_DIV); - MotorData[MotorNr].OldPositionError = PositionError; + DValue = (PositionError - pMD->OldPositionError) * (SWORD)(pMD->RegDParameter/REG_CONST_DIV); + pMD->OldPositionError = PositionError; - MotorData[MotorNr].AccError = (MotorData[MotorNr].AccError * 3) + PositionError; - MotorData[MotorNr].AccError = MotorData[MotorNr].AccError / 4; + pMD->AccError = (pMD->AccError * 3) + PositionError; + pMD->AccError = pMD->AccError / 4; - if (MotorData[MotorNr].AccError > (SWORD)800) + if (pMD->AccError > (SWORD)800) { - MotorData[MotorNr].AccError = 800; + pMD->AccError = 800; } - if (MotorData[MotorNr].AccError <= (SWORD)-800) + if (pMD->AccError <= (SWORD)-800) { - MotorData[MotorNr].AccError = -800; + pMD->AccError = -800; } - IValue = MotorData[MotorNr].AccError * (SWORD)(MotorData[MotorNr].RegIParameter/REG_CONST_DIV); + IValue = pMD->AccError * (SWORD)(pMD->RegIParameter/REG_CONST_DIV); if (IValue > (SWORD)REG_MAX_VALUE) { @@ -841,14 +896,14 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) if (TotalRegValue > MAXIMUM_SPEED_FW) { TotalRegValue = MAXIMUM_SPEED_FW; - MotorData[MotorNr].MotorOverloaded = 1; + pMD->MotorOverloaded = 1; } if (TotalRegValue < MAXIMUM_SPEED_RW) { TotalRegValue = MAXIMUM_SPEED_RW; - MotorData[MotorNr].MotorOverloaded = 1; + pMD->MotorOverloaded = 1; } - MotorData[MotorNr].MotorActualSpeed = (SBYTE)TotalRegValue; + pMD->MotorActualSpeed = (SBYTE)TotalRegValue; } /* Regulation function used when syncrhonization regulation is enabled */ @@ -862,38 +917,40 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) SWORD CorrectionValue; SWORD MotorSpeed; - SyncData.SyncTachoDif = (SLONG)((MotorData[MotorOne].MotorBlockTachoCount) - (MotorData[MotorTwo].MotorBlockTachoCount)); + MOTORDATA * pOne = &(MotorData[MotorOne]); + MOTORDATA * pTwo = &(MotorData[MotorTwo]); + SyncData.SyncTachoDif = (SLONG)((pOne->MotorBlockTachoCount) - (pTwo->MotorBlockTachoCount)); - if (MotorData[MotorOne].TurnParameter != 0) + if (pOne->TurnParameter != 0) { - if ((MotorData[MotorOne].MotorBlockTachoCount != 0) || (MotorData[MotorTwo].MotorBlockTachoCount)) + if ((pOne->MotorBlockTachoCount != 0) || (pTwo->MotorBlockTachoCount != 0)) { - if (MotorData[MotorOne].MotorTargetSpeed >= 0) + if (pOne->MotorTargetSpeed >= 0) { - if (MotorData[MotorOne].TurnParameter > 0) + if (pOne->TurnParameter > 0) { - TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorTwo].TurnParameter * (SLONG)MotorData[MotorTwo].MotorTargetSpeed)/100); + TempTurnParameter = (SLONG)(((SLONG)pTwo->TurnParameter * (SLONG)pTwo->MotorTargetSpeed)/100); } else { - TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorOne].TurnParameter * (SLONG)MotorData[MotorOne].MotorTargetSpeed)/100); + TempTurnParameter = (SLONG)(((SLONG)pOne->TurnParameter * (SLONG)pOne->MotorTargetSpeed)/100); } } else { - if (MotorData[MotorOne].TurnParameter > 0) + if (pOne->TurnParameter > 0) { - TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorOne].TurnParameter * (-(SLONG)MotorData[MotorOne].MotorTargetSpeed))/100); + TempTurnParameter = (SLONG)(((SLONG)pOne->TurnParameter * (-(SLONG)pOne->MotorTargetSpeed))/100); } else { - TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorTwo].TurnParameter * (-(SLONG)MotorData[MotorTwo].MotorTargetSpeed))/100); + TempTurnParameter = (SLONG)(((SLONG)pTwo->TurnParameter * (-(SLONG)pTwo->MotorTargetSpeed))/100); } } } else { - TempTurnParameter = MotorData[MotorOne].TurnParameter; + TempTurnParameter = pOne->TurnParameter; } } else @@ -906,16 +963,6 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) 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; @@ -924,11 +971,10 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) { SyncData.SyncTachoDif = -500; } - */ - PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(MotorData[MotorOne].RegPParameter/REG_CONST_DIV); + PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV); - DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(MotorData[MotorOne].RegDParameter/REG_CONST_DIV); + DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(pOne->RegDParameter/REG_CONST_DIV); SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif; SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif; @@ -941,11 +987,11 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) { SyncData.SyncAccError = -900; } - IValue = SyncData.SyncAccError * (SWORD)(MotorData[MotorOne].RegIParameter/REG_CONST_DIV); + IValue = SyncData.SyncAccError * (SWORD)(pOne->RegIParameter/REG_CONST_DIV); CorrectionValue = (SWORD)((PValue + IValue + DValue)/4); - MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed - CorrectionValue; + MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue; if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW) { @@ -959,40 +1005,40 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) } } - if (MotorData[MotorOne].TurnParameter != 0) + if (pOne->TurnParameter != 0) { - if (MotorData[MotorOne].MotorTargetSpeed > 0) + if (pOne->MotorTargetSpeed > 0) { - if (MotorSpeed > (SWORD)MotorData[MotorOne].MotorTargetSpeed) + if (MotorSpeed > (SWORD)pOne->MotorTargetSpeed) { - MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed; + MotorSpeed = (SWORD)pOne->MotorTargetSpeed; } else { - if (MotorSpeed < (SWORD)-MotorData[MotorOne].MotorTargetSpeed) + if (MotorSpeed < (SWORD)-pOne->MotorTargetSpeed) { - MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed; + MotorSpeed = -pOne->MotorTargetSpeed; } } } else { - if (MotorSpeed < (SWORD)MotorData[MotorOne].MotorTargetSpeed) + if (MotorSpeed < (SWORD)pOne->MotorTargetSpeed) { - MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed; + MotorSpeed = (SWORD)pOne->MotorTargetSpeed; } else { - if (MotorSpeed > (SWORD)-MotorData[MotorOne].MotorTargetSpeed) + if (MotorSpeed > (SWORD)-pOne->MotorTargetSpeed) { - MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed; + MotorSpeed = -pOne->MotorTargetSpeed; } } } } - MotorData[MotorOne].MotorActualSpeed = (SBYTE)MotorSpeed; + pOne->MotorActualSpeed = (SBYTE)MotorSpeed; - MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed + CorrectionValue; + MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue; if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW) { @@ -1006,47 +1052,71 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) } } - if (MotorData[MotorOne].TurnParameter != 0) + if (pOne->TurnParameter != 0) { - if (MotorData[MotorTwo].MotorTargetSpeed > 0) + if (pTwo->MotorTargetSpeed > 0) { - if (MotorSpeed > (SWORD)MotorData[MotorTwo].MotorTargetSpeed) + if (MotorSpeed > (SWORD)pTwo->MotorTargetSpeed) { - MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed; + MotorSpeed = (SWORD)pTwo->MotorTargetSpeed; } else { - if (MotorSpeed < (SWORD)-MotorData[MotorTwo].MotorTargetSpeed) + if (MotorSpeed < (SWORD)-pTwo->MotorTargetSpeed) { - MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed; + MotorSpeed = -pTwo->MotorTargetSpeed; } } } else { - if (MotorSpeed < (SWORD)MotorData[MotorTwo].MotorTargetSpeed) + if (MotorSpeed < (SWORD)pTwo->MotorTargetSpeed) { - MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed; + MotorSpeed = (SWORD)pTwo->MotorTargetSpeed; } else { - if (MotorSpeed > (SWORD)-MotorData[MotorTwo].MotorTargetSpeed) + if (MotorSpeed > (SWORD)-pTwo->MotorTargetSpeed) { - MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed; + MotorSpeed = -pTwo->MotorTargetSpeed; } } } } - MotorData[MotorTwo].MotorActualSpeed = (SBYTE)MotorSpeed; + pTwo->MotorActualSpeed = (SBYTE)MotorSpeed; } //Called when the motor is ramping down void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { - UBYTE MotorOne, MotorTwo; - - if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) + MOTORDATA * pOne = &(MotorData[MotorNr]); + if (pOne->RegulationMode & REGSTATE_SYNCHRONE) { + UBYTE MotorOne, MotorTwo; + MotorOne = MotorNr; + MotorTwo = 0xFF; + for(UBYTE 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 = pOne->RunStateAtLimit; + pOne->RegulationMode = REGSTATE_IDLE; + if (MotorTwo != 0xFF) { + MOTORDATA * pTwo = &(MotorData[MotorTwo]); + pTwo->MotorSetSpeed = 0; + pTwo->MotorTargetSpeed = 0; + pTwo->MotorActualSpeed = 0; + pTwo->MotorRunState = pTwo->RunStateAtLimit; + pTwo->RegulationMode = REGSTATE_IDLE; + } +/* if (MotorNr == MOTOR_A) { MotorOne = MotorNr; @@ -1055,14 +1125,15 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { //Synchronise motor A & B MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; + MotorData[MotorOne].MotorTargetSpeed = 0; MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorOne].RegulationMode = REGSTATE_IDLE; + MotorData[MotorTwo].MotorSetSpeed = 0; - MotorData[MotorTwo].MotorTargetSpeed = 0; + MotorData[MotorTwo].MotorTargetSpeed = 0; MotorData[MotorTwo].MotorActualSpeed = 0; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; } else @@ -1072,23 +1143,23 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) { //Synchronise motor A & C MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; + MotorData[MotorOne].MotorTargetSpeed = 0; MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorOne].RegulationMode = REGSTATE_IDLE; MotorData[MotorTwo].MotorSetSpeed = 0; - MotorData[MotorTwo].MotorTargetSpeed = 0; + MotorData[MotorTwo].MotorTargetSpeed = 0; MotorData[MotorTwo].MotorActualSpeed = 0; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; } else { //Only Motor A has Sync setting => Stop normal MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; + MotorData[MotorNr].MotorTargetSpeed = 0; MotorData[MotorNr].MotorActualSpeed = 0; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; } } @@ -1103,12 +1174,12 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) MotorData[MotorOne].MotorSetSpeed = 0; MotorData[MotorOne].MotorTargetSpeed = 0; MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit; 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].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; } MotorTwo = MotorOne + 1; @@ -1118,12 +1189,12 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) MotorData[MotorOne].MotorSetSpeed = 0; MotorData[MotorOne].MotorTargetSpeed = 0; MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit; 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].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; } else @@ -1132,7 +1203,7 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) MotorData[MotorNr].MotorSetSpeed = 0; MotorData[MotorNr].MotorTargetSpeed = 0; MotorData[MotorNr].MotorActualSpeed = 0; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; } } @@ -1146,12 +1217,12 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) MotorData[MotorOne].MotorSetSpeed = 0; MotorData[MotorOne].MotorTargetSpeed = 0; MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit; 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].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; } MotorTwo = MotorOne - 1; @@ -1161,35 +1232,35 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr) MotorData[MotorOne].MotorSetSpeed = 0; MotorData[MotorOne].MotorTargetSpeed = 0; MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorOne].MotorRunState = pOne->RunStateAtLimit; 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].MotorRunState = pTwo->RunStateAtLimit; MotorData[MotorTwo].RegulationMode = REGSTATE_IDLE; } else { //Only Motor C has Sync settings => Stop normal MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; + MotorData[MotorNr].MotorTargetSpeed = 0; MotorData[MotorNr].MotorActualSpeed = 0; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; + MotorData[MotorNr].MotorRunState = pOne->RunStateAtLimit; MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; } } +*/ } else { - if (MotorData[MotorNr].MotorSetSpeed == 0) + if (pOne->MotorSetSpeed == 0) { - MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; - MotorData[MotorNr].MotorActualSpeed = 0; + pOne->MotorTargetSpeed = 0; + pOne->MotorActualSpeed = 0; } - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; + pOne->MotorRunState = pOne->RunStateAtLimit; + pOne->RegulationMode = REGSTATE_IDLE; } } @@ -1199,6 +1270,19 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr) { UBYTE MotorOne, MotorTwo; + MotorOne = MotorNr; + MotorTwo = 0xFF; + for(UBYTE 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 (MotorNr == MOTOR_A) { MotorOne = MotorNr; @@ -1274,80 +1358,97 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr) } } } - +*/ if ((MotorOne != 0xFF) && (MotorTwo != 0xFF)) { - if (MotorData[MotorOne].TurnParameter != 0) + MOTORDATA * pOne = &(MotorData[MotorOne]); + MOTORDATA * pTwo = &(MotorData[MotorTwo]); + SLONG l1 = pOne->MotorTachoCountToRun; + SLONG l2 = pTwo->MotorTachoCountToRun; + UBYTE NewRunState1 = pOne->RunStateAtLimit; + UBYTE NewRunState2 = pTwo->RunStateAtLimit; + if (pOne->RampDownToLimit == OPTION_RAMPDOWNTOLIMIT) + { + NewRunState1 = MOTOR_RUN_STATE_RAMPDOWN; + l1 = (SLONG)((float)l1 * 0.80f); + } + if (pTwo->RampDownToLimit == OPTION_RAMPDOWNTOLIMIT) + { + NewRunState2 = MOTOR_RUN_STATE_RAMPDOWN; + l2 = (SLONG)((float)l2 * 0.80f); + } + if (pOne->TurnParameter != 0) { - if (MotorData[MotorOne].TurnParameter > 0) + if (pOne->TurnParameter > 0) { - if (MotorData[MotorTwo].MotorTargetSpeed >= 0) + if (pTwo->MotorTargetSpeed >= 0) { - if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun)) + if ((SLONG)(pTwo->CurrentCaptureCount >= l2)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = NewRunState1; + pTwo->MotorRunState = NewRunState2; - MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount; - MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun; + pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount; + pOne->MotorTachoCountToRun = l2; } } else { - if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun)) + if ((SLONG)(pOne->CurrentCaptureCount <= l1)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = NewRunState1; + pTwo->MotorRunState = NewRunState2; - MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount; - MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun; + pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount; + pTwo->MotorTachoCountToRun = l1; } } } else { - if (MotorData[MotorOne].MotorTargetSpeed >= 0) + if (pOne->MotorTargetSpeed >= 0) { - if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun)) + if ((SLONG)(pOne->CurrentCaptureCount >= l1)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = NewRunState1; + pTwo->MotorRunState = NewRunState2; - MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount; - MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun; + pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount; + pTwo->MotorTachoCountToRun = l1; } } else { - if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun)) + if ((SLONG)(pTwo->CurrentCaptureCount <= l2)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = NewRunState1; + pTwo->MotorRunState = NewRunState2; - MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount; - MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun; + pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount; + pOne->MotorTachoCountToRun = l2; } } } } else { - if (MotorData[MotorOne].MotorSetSpeed > 0) + // no turning + if (pOne->MotorSetSpeed > 0) { - if ((MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun) || (MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun)) + if ((pOne->CurrentCaptureCount >= l1) || (pTwo->CurrentCaptureCount >= l2)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = NewRunState1; + pTwo->MotorRunState = NewRunState2; } } else { - if (MotorData[MotorOne].MotorSetSpeed < 0) + if (pOne->MotorSetSpeed < 0) { - if ((MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun) || (MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun)) + if ((pOne->CurrentCaptureCount <= l1) || (pTwo->CurrentCaptureCount <= l2)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = NewRunState1; + pTwo->MotorRunState = NewRunState2; } } } @@ -1411,6 +1512,19 @@ void dOutputResetSyncMotors(UBYTE MotorNr) { UBYTE MotorOne, MotorTwo; + MotorOne = MotorNr; + MotorTwo = 0xFF; + for(UBYTE 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 (MotorNr == MOTOR_A) { MotorOne = MotorNr; @@ -1480,18 +1594,20 @@ void dOutputResetSyncMotors(UBYTE MotorNr) } } } - +*/ + MOTORDATA * pMD = &(MotorData[MotorNr]); if ((MotorOne != 0xFF) && (MotorTwo != 0xFF)) { - MotorData[MotorOne].CurrentCaptureCount = 0; - MotorData[MotorOne].MotorTachoCountToRun = 0; - MotorData[MotorTwo].CurrentCaptureCount = 0; - MotorData[MotorTwo].MotorTachoCountToRun = 0; + MOTORDATA * pTwo = &(MotorData[MotorTwo]); + pMD->CurrentCaptureCount = 0; + pMD->MotorTachoCountToRun = 0; + pTwo->CurrentCaptureCount = 0; + pTwo->MotorTachoCountToRun = 0; } else { - MotorData[MotorNr].CurrentCaptureCount = 0; - MotorData[MotorNr].MotorTachoCountToRun = 0; + pMD->CurrentCaptureCount = 0; + pMD->MotorTachoCountToRun = 0; } } @@ -1500,6 +1616,19 @@ void dOutputRampDownSynch(UBYTE MotorNr) { UBYTE MotorOne, MotorTwo; + MotorOne = MotorNr; + MotorTwo = 0xFF; + for(UBYTE 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 (MotorNr == MOTOR_A) { MotorOne = MotorNr; @@ -1571,42 +1700,44 @@ void dOutputRampDownSynch(UBYTE MotorNr) } } } - +*/ if ((MotorOne != 0xFF) && (MotorTwo != 0xFF)) { - if (MotorData[MotorOne].TurnParameter != 0) + MOTORDATA * pOne = &(MotorData[MotorOne]); + MOTORDATA * pTwo = &(MotorData[MotorTwo]); + if (pOne->TurnParameter != 0) { - if (MotorData[MotorOne].TurnParameter > 0) + if (pOne->TurnParameter > 0) { - if (MotorData[MotorOne].MotorTargetSpeed >= 0) + if (pOne->MotorTargetSpeed >= 0) { - if (MotorData[MotorTwo].MotorActualSpeed < 0) + if (pTwo->MotorActualSpeed < 0) { - MotorData[MotorTwo].MotorTargetSpeed--; + pTwo->MotorTargetSpeed--; } } else { - if (MotorData[MotorTwo].MotorActualSpeed > 0) + if (pTwo->MotorActualSpeed > 0) { - MotorData[MotorTwo].MotorTargetSpeed++; + pTwo->MotorTargetSpeed++; } } } else { - if (MotorData[MotorOne].MotorTargetSpeed >= 0) + if (pOne->MotorTargetSpeed >= 0) { - if (MotorData[MotorTwo].MotorActualSpeed < 0) + if (pTwo->MotorActualSpeed < 0) { - MotorData[MotorTwo].MotorTargetSpeed--; + pTwo->MotorTargetSpeed--; } } else { - if (MotorData[MotorTwo].MotorActualSpeed > 0) + if (pTwo->MotorActualSpeed > 0) { - MotorData[MotorTwo].MotorTargetSpeed++; + pTwo->MotorTargetSpeed++; } } } @@ -1614,3 +1745,7 @@ void dOutputRampDownSynch(UBYTE MotorNr) } } +void dOutputUpdateRegulationTime(UBYTE rt) +{ + RegTime = rt; +} -- cgit v1.2.3