From 13f8dd7556fd756722cd669c1f5d5b9311a49777 Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 00:44:37 +0100 Subject: replace many array indexes with pointer access Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_output.c | 141 +++++++++++++++++++++-------------------- 1 file changed, 72 insertions(+), 69 deletions(-) (limited to 'AT91SAM7S256/Source/c_output.c') diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index 9566938..e2115a7 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -45,15 +45,16 @@ void cOutputInit(void* pHeader) 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; } VarsOutput.TimeCnt = 0; dOutputInit(); @@ -65,72 +66,73 @@ void cOutputCtrl(void) 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) + { + pOut->Flags &= ~UPDATE_RESET_BLOCK_COUNT; + dOutputResetBlockTachoLimit(Tmp); + } + if (pOut->Flags & UPDATE_SPEED) { - IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_RESET_BLOCK_COUNT; - dOutputResetBlockTachoLimit(Tmp); + pOut->Flags &= ~UPDATE_SPEED; + if (pOut->Mode & MOTORON) + { + dOutputSetSpeed(Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter); + } } - 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_TACHO_LIMIT) + { + pOut->Flags &= ~UPDATE_TACHO_LIMIT; + dOutputSetTachoLimit(Tmp, pOut->TachoLimit); + } + 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 (IOMapOutput.Outputs[Tmp].Mode & REGULATED) - { - dOutputEnableRegulation(Tmp, IOMapOutput.Outputs[Tmp].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->Mode & MOTORON) + { + if (pOut->Mode & REGULATED) + { + dOutputEnableRegulation(Tmp, pOut->RegMode); + } + else + { + dOutputDisableRegulation(Tmp); + } + } + else + { + dOutputSetSpeed(Tmp, 0x00, 0x00, 0x00); + dOutputDisableRegulation(Tmp); + } + } + if (pOut->Flags & UPDATE_PID_VALUES) + { + pOut->Flags &= ~UPDATE_PID_VALUES; + dOutputSetPIDParameters(Tmp, pOut->RegPParameter, pOut->RegIParameter, pOut->RegDParameter); + } + } } dOutputCtrl(); cOutputUpdateIomap(); @@ -151,14 +153,15 @@ void cOutputUpdateIomap(void) 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]; } } } -- cgit v1.2.3 From 43549170a241b509cdc00f1c629c9d541ff2588d Mon Sep 17 00:00:00 2001 From: John Hansen Date: Thu, 27 Jan 2011 20:00:43 +0100 Subject: add output options Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_cmd.c | 5 +++++ AT91SAM7S256/Source/c_cmd.h | 5 +++-- AT91SAM7S256/Source/c_output.c | 1 + AT91SAM7S256/Source/c_output.iom | 2 +- 4 files changed, 10 insertions(+), 3 deletions(-) (limited to 'AT91SAM7S256/Source/c_output.c') diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index 6f562ca..5370652 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -266,6 +266,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = TC_UBYTE, //IO_OUT_REG_D_VAL TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT TC_SLONG, //IO_OUT_ROTATION_COUNT + TC_UBYTE, //IO_OUT_OPTIONS //IO_OUT1 TC_UBYTE, //IO_OUT_FLAGS @@ -283,6 +284,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = TC_UBYTE, //IO_OUT_REG_D_VAL TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT TC_SLONG, //IO_OUT_ROTATION_COUNT + TC_UBYTE, //IO_OUT_OPTIONS //IO_OUT2 TC_UBYTE, //IO_OUT_FLAGS @@ -300,6 +302,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = TC_UBYTE, //IO_OUT_REG_D_VAL TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT TC_SLONG, //IO_OUT_ROTATION_COUNT + TC_UBYTE, //IO_OUT_OPTIONS }; @@ -486,6 +489,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) pOut->RegMode = pInBuf[4]; pOut->SyncTurnParameter = pInBuf[5]; pOut->RunState = pInBuf[6]; + pOut->Options = pOut->Mode & REG_METHOD; memcpy((PSZ)(&(pOut->TachoLimit)), (PSZ)(&pInBuf[7]), 4); pOut->Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT; @@ -1107,6 +1111,7 @@ void cCmdInit(void* pHeader) IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegDParameter); IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->BlockTachoCount); IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->RotationCount); + IO_PTRS_OUT[IO_OUT_OPTIONS + i * IO_OUT_FPP] = (void*)&(pOut->Options); } //Initialize IO_PTRS_IN diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h index e9227eb..acd8676 100644 --- a/AT91SAM7S256/Source/c_cmd.h +++ b/AT91SAM7S256/Source/c_cmd.h @@ -194,10 +194,11 @@ enum IO_OUT_REG_I_VAL, IO_OUT_REG_D_VAL, IO_OUT_BLOCK_TACH_COUNT, - IO_OUT_ROTATION_COUNT + IO_OUT_ROTATION_COUNT, + IO_OUT_OPTIONS, }; -#define IO_OUT_FPP 15 +#define IO_OUT_FPP 16 #define IO_OUT_FIELD_COUNT (IO_OUT_FPP * NO_OF_OUTPUTS) // diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index e2115a7..4601ff3 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -55,6 +55,7 @@ void cOutputInit(void* pHeader) 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(); diff --git a/AT91SAM7S256/Source/c_output.iom b/AT91SAM7S256/Source/c_output.iom index 80e35de..fc26efd 100644 --- a/AT91SAM7S256/Source/c_output.iom +++ b/AT91SAM7S256/Source/c_output.iom @@ -72,7 +72,7 @@ typedef struct UBYTE RegMode; /* RW - Tells which regulation mode should be used */ UBYTE Overloaded; /* R - True if the motor has been overloaded within speed control regulation */ SBYTE SyncTurnParameter; /* RW - Holds the turning parameter need within MoveBlock */ - UBYTE SpareOne; + UBYTE Options; UBYTE SpareTwo; UBYTE SpareThree; }OUTPUT; -- cgit v1.2.3