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/c_output.c | 152 +++++++++++++++++++++-------------------- 1 file changed, 79 insertions(+), 73 deletions(-) (limited to 'AT91SAM7S256/Source/c_output.c') diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index 9566938..23e0e28 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -43,17 +43,20 @@ void cOutputInit(void* pHeader) { UBYTE Tmp; + IOMapOutput.PwnFreq = REGULATION_TIME; for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) { - IOMapOutput.Outputs[Tmp].Mode = 0x00; - IOMapOutput.Outputs[Tmp].Speed = 0x00; - IOMapOutput.Outputs[Tmp].ActualSpeed = 0x00; - IOMapOutput.Outputs[Tmp].TachoCnt = 0x00; - IOMapOutput.Outputs[Tmp].RunState = 0x00; - IOMapOutput.Outputs[Tmp].TachoLimit = 0x00; - IOMapOutput.Outputs[Tmp].RegPParameter = DEFAULT_P_GAIN_FACTOR; - IOMapOutput.Outputs[Tmp].RegIParameter = DEFAULT_I_GAIN_FACTOR; - IOMapOutput.Outputs[Tmp].RegDParameter = DEFAULT_D_GAIN_FACTOR; + OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); + pOut->Mode = 0x00; + pOut->Speed = 0x00; + pOut->ActualSpeed = 0x00; + pOut->TachoCnt = 0x00; + pOut->RunState = 0x00; + pOut->TachoLimit = 0x00; + pOut->RegPParameter = DEFAULT_P_GAIN_FACTOR; + pOut->RegIParameter = DEFAULT_I_GAIN_FACTOR; + pOut->RegDParameter = DEFAULT_D_GAIN_FACTOR; + pOut->Options = 0x00; } VarsOutput.TimeCnt = 0; dOutputInit(); @@ -63,74 +66,76 @@ void cOutputCtrl(void) { UBYTE Tmp; + dOutputUpdateRegulationTime(IOMapOutput.PwnFreq); for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) { - if (IOMapOutput.Outputs[Tmp].Flags != 0) - { - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_RESET_ROTATION_COUNT) + OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); + if (pOut->Flags != 0) + { + if (pOut->Flags & UPDATE_RESET_ROTATION_COUNT) { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_ROTATION_COUNT; + pOut->Flags &= ~UPDATE_RESET_ROTATION_COUNT; dOutputResetRotationCaptureCount(Tmp); } - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_RESET_COUNT) - { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_COUNT; - dOutputResetTachoLimit(Tmp); - } - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_RESET_BLOCK_COUNT) + if (pOut->Flags & UPDATE_RESET_COUNT) + { + pOut->Flags &= ~UPDATE_RESET_COUNT; + dOutputResetTachoLimit(Tmp); + } + if (pOut->Flags & UPDATE_RESET_BLOCK_COUNT) { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_BLOCK_COUNT; - dOutputResetBlockTachoLimit(Tmp); + pOut->Flags &= ~UPDATE_RESET_BLOCK_COUNT; + dOutputResetBlockTachoLimit(Tmp); } - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_SPEED) - { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_SPEED; - if (IOMapOutput.Outputs[Tmp].Mode & MOTORON) - { - dOutputSetSpeed (Tmp, IOMapOutput.Outputs[Tmp].RunState, IOMapOutput.Outputs[Tmp].Speed, IOMapOutput.Outputs[Tmp].SyncTurnParameter); - } - } - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_TACHO_LIMIT) - { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_TACHO_LIMIT; - dOutputSetTachoLimit(Tmp, IOMapOutput.Outputs[Tmp].TachoLimit); - } - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_MODE) - { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_MODE; - if (IOMapOutput.Outputs[Tmp].Mode & BRAKE) + if (pOut->Flags & UPDATE_SPEED) + { + pOut->Flags &= ~UPDATE_SPEED; + if (pOut->Mode & MOTORON) + { + dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter); + } + } + if (pOut->Flags & UPDATE_TACHO_LIMIT) + { + pOut->Flags &= ~UPDATE_TACHO_LIMIT; + dOutputSetTachoLimit(Tmp, pOut->TachoLimit, pOut->Options); + } + if (pOut->Flags & UPDATE_MODE) + { + pOut->Flags &= ~UPDATE_MODE; + if (pOut->Mode & BRAKE) { // Motor is Braked dOutputSetMode(Tmp, BRAKE); - } + } else { // Motor is floated dOutputSetMode(Tmp, 0x00); } - if (IOMapOutput.Outputs[Tmp].Mode & MOTORON) + if (pOut->Mode & MOTORON) { - if (IOMapOutput.Outputs[Tmp].Mode & REGULATED) - { - dOutputEnableRegulation(Tmp, IOMapOutput.Outputs[Tmp].RegMode); - } - else - { - dOutputDisableRegulation(Tmp); - } - } - else - { - dOutputSetSpeed(Tmp, 0x00, 0x00, 0x00); + if (pOut->Mode & REGULATED) + { + dOutputEnableRegulation(Tmp, pOut->RegMode); + } + else + { + dOutputDisableRegulation(Tmp); + } + } + else + { + dOutputSetSpeed(Tmp, 0x00, 0x00, 0x00); dOutputDisableRegulation(Tmp); - } - } - if (IOMapOutput.Outputs[Tmp].Flags & UPDATE_PID_VALUES) - { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_PID_VALUES; - dOutputSetPIDParameters(Tmp, IOMapOutput.Outputs[Tmp].RegPParameter, IOMapOutput.Outputs[Tmp].RegIParameter, IOMapOutput.Outputs[Tmp].RegDParameter); - } - } + } + } + if (pOut->Flags & UPDATE_PID_VALUES) + { + pOut->Flags &= ~UPDATE_PID_VALUES; + dOutputSetPIDParameters(Tmp, pOut->RegPParameter, pOut->RegIParameter, pOut->RegDParameter); + } + } } dOutputCtrl(); cOutputUpdateIomap(); @@ -138,29 +143,30 @@ void cOutputCtrl(void) void cOutputUpdateIomap(void) { - UBYTE TempCurrentMotorSpeed[NO_OF_OUTPUTS]; - UBYTE TempRunState[NO_OF_OUTPUTS]; + UBYTE TempCurrentMotorSpeed[NO_OF_OUTPUTS]; + UBYTE TempRunState[NO_OF_OUTPUTS]; UBYTE TempMotorOverloaded[NO_OF_OUTPUTS]; - SLONG TempTachoCount[NO_OF_OUTPUTS]; + SLONG TempTachoCount[NO_OF_OUTPUTS]; SLONG TempBlockTachoCount[NO_OF_OUTPUTS]; SLONG TempRotationCount[NO_OF_OUTPUTS]; UBYTE Tmp; - dOutputGetMotorParameters(TempCurrentMotorSpeed, TempTachoCount, TempBlockTachoCount, TempRunState, TempMotorOverloaded,TempRotationCount); + dOutputGetMotorParameters(TempCurrentMotorSpeed, TempTachoCount, TempBlockTachoCount, TempRunState, TempMotorOverloaded,TempRotationCount); - for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) + for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++) { - IOMapOutput.Outputs[Tmp].ActualSpeed = TempCurrentMotorSpeed[Tmp]; - IOMapOutput.Outputs[Tmp].TachoCnt = TempTachoCount[Tmp]; - IOMapOutput.Outputs[Tmp].BlockTachoCount = TempBlockTachoCount[Tmp]; - IOMapOutput.Outputs[Tmp].RotationCount = TempRotationCount[Tmp]; - IOMapOutput.Outputs[Tmp].Overloaded = TempMotorOverloaded[Tmp]; - if (!(IOMapOutput.Outputs[Tmp].Flags & PENDING_UPDATES)) + OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); + pOut->ActualSpeed = TempCurrentMotorSpeed[Tmp]; + pOut->TachoCnt = TempTachoCount[Tmp]; + pOut->BlockTachoCount = TempBlockTachoCount[Tmp]; + pOut->RotationCount = TempRotationCount[Tmp]; + pOut->Overloaded = TempMotorOverloaded[Tmp]; + if (!(pOut->Flags & PENDING_UPDATES)) { - IOMapOutput.Outputs[Tmp].RunState = TempRunState[Tmp]; + pOut->RunState = TempRunState[Tmp]; } - } + } } void cOutputExit(void) -- cgit v1.2.3