From 297e078de44336eb37e59c31638988de4adfb88b Mon Sep 17 00:00:00 2001 From: John Hansen Date: Mon, 29 Nov 2010 13:26:57 +0100 Subject: add support for native executable Enable the use of native compiled program from standard firmware interface. See http://www.tau.ac.il/~stoledo/lego/nxt-native/ for more details. Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_cmd.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index 4ce9829..7483846 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -1425,6 +1425,16 @@ NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, NXT_ASSERT(pData != NULL); + if (strncmp((PSZ)pData, "NXTBINARY", VM_FORMAT_STRING_SIZE) == 0) + { + ULONG NativeOffset; + pCursor = (pData + 12); + NativeOffset = (ULONG)(*pCursor); + void (*native)(ULONG, ULONG) = (void (*)())(pData + NativeOffset); + (*native)((ULONG)pData, DataSize); + NXT_BREAK; + return (ERR_VER); + } //Assign pCursor to point to version word inside file header pCursor = (pData + VM_FORMAT_STRING_SIZE - 2); -- cgit v1.2.3 From d6bd4ec76414e535a297e37aef9de066c20b7e10 Mon Sep 17 00:00:00 2001 From: John Hansen Date: Thu, 20 Jan 2011 01:27:54 +0100 Subject: use float instead of double when possible Useless use of double precision math will include large functions and eat CPU time. Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/Functions.inl | 30 +++++++++++++++--------------- AT91SAM7S256/Source/c_cmd.c | 22 +++++++++++----------- AT91SAM7S256/Source/c_ui.h | 2 +- 3 files changed, 27 insertions(+), 27 deletions(-) diff --git a/AT91SAM7S256/Source/Functions.inl b/AT91SAM7S256/Source/Functions.inl index 4211137..fb35f9a 100644 --- a/AT91SAM7S256/Source/Functions.inl +++ b/AT91SAM7S256/Source/Functions.inl @@ -83,19 +83,19 @@ const UBYTE SENSORFORMAT[SENSORS][9] = const float SENSORDIVIDER[SENSORS] = { - 1.0, // MENU_SENSOR_EMPTY - 1.0, // MENU_SENSOR_SOUND_DB - 1.0, // MENU_SENSOR_SOUND_DBA - 1.0, // MENU_SENSOR_LIGHT - 1.0, // MENU_SENSOR_LIGHT_AMB - 1.0, // MENU_SENSOR_TOUCH - 1.0, // MENU_SENSOR_MOTOR_DEG - 360.0, // MENU_SENSOR_MOTOR_ROT - 2.54, // MENU_SENSOR_ULTRASONIC_IN - 1.0, // MENU_SENSOR_ULTRASONIC_CM - 10.0, // MENU_SENSOR_IIC_TEMP_C - 10.0, // MENU_SENSOR_IIC_TEMP_F - 1.0 // MENU_SENSOR_COLOR + 1.0f, // MENU_SENSOR_EMPTY + 1.0f, // MENU_SENSOR_SOUND_DB + 1.0f, // MENU_SENSOR_SOUND_DBA + 1.0f, // MENU_SENSOR_LIGHT + 1.0f, // MENU_SENSOR_LIGHT_AMB + 1.0f, // MENU_SENSOR_TOUCH + 1.0f, // MENU_SENSOR_MOTOR_DEG + 360.0f, // MENU_SENSOR_MOTOR_ROT + 2.54f, // MENU_SENSOR_ULTRASONIC_IN + 1.0f, // MENU_SENSOR_ULTRASONIC_CM + 10.0f, // MENU_SENSOR_IIC_TEMP_C + 10.0f, // MENU_SENSOR_IIC_TEMP_F + 1.0f // MENU_SENSOR_COLOR }; @@ -322,11 +322,11 @@ void cUiUpdateSensor(SWORD Time) { if (Sensor == MENU_SENSOR_IIC_TEMP_F) { - VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)((float)(Tmp + 4544) / 14.2); + VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)((float)(Tmp + 4544) / 14.2f); } else { - VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)((float)Tmp / 25.6); + VarsUi.DatalogSampleValue[Port - MENU_PORT_1] = (SLONG)((float)Tmp / 25.6f); } VarsUi.DatalogSampleValid[Port - MENU_PORT_1] = TRUE; } diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index 7483846..fc74b3a 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -3436,11 +3436,11 @@ ULONG cCmdGetSByte(void * pVal) { ULONG cCmdGetFloat(void * pVal) { float tempVal = *(float*)pVal; - if (tempVal >= 0) { - tempVal += 0.5; + if (tempVal >= 0.0f) { + tempVal += 0.5f; } else { - tempVal -= 0.5; + tempVal -= 0.5f; } return (ULONG)tempVal; } @@ -4613,7 +4613,7 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2); // is number too big for display? then format differently and don't bother with trailing zeros - if ((FltArgVal2 > 9999999999999.99)||(FltArgVal2 < -999999999999.99)){ // these are the widest %.2f numbers that will fit on display + if ((FltArgVal2 > 9999999999999.99f)||(FltArgVal2 < -999999999999.99f)){ // these are the widest %.2f numbers that will fit on display strcpy (FormatString, "%.6g"); } else{ @@ -4951,20 +4951,20 @@ float cCmdUnop2Flt(CODE_WORD const Code, float Operand, TYPE_CODE TypeCode) else if(opCode == OP_ABS) return fabsf(Operand); else if(opCode == OP_SQRT) - return sqrt(Operand); + return sqrtf(Operand); #if 0 else if(opCode == OP_SIN) - return sin(Operand); + return sinf(Operand); else if(opCode == OP_COS) - return cos(Operand); + return cosf(Operand); else if(opCode == OP_TAN) - return tan(Operand); + return tanf(Operand); else if(opCode == OP_ASIN) - return asin(Operand); + return asinf(Operand); else if(opCode == OP_ACOS) - return acos(Operand); + return acosf(Operand); else if(opCode == OP_ATAN) - return atan(Operand); + return atanf(Operand); #endif else { diff --git a/AT91SAM7S256/Source/c_ui.h b/AT91SAM7S256/Source/c_ui.h index e74dcbe..e0f8f4a 100644 --- a/AT91SAM7S256/Source/c_ui.h +++ b/AT91SAM7S256/Source/c_ui.h @@ -36,7 +36,7 @@ #define CHECKBYTE 0x78 // Used to validate NVData -#define BATTERY_COUNT_TO_MV 13.848 // Battery count to mV factor [mV/cnt] +#define BATTERY_COUNT_TO_MV 13.848f // Battery count to mV factor [mV/cnt] #define LOW_BATT_THRESHOLD 6 // Low batt conunts before warning #define BUTTON_DELAY_TIME 800 // Delay before first repeat [mS] -- cgit v1.2.3 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_cmd.c | 213 ++++++++++++++++++++++------------------- AT91SAM7S256/Source/c_input.c | 19 ++-- AT91SAM7S256/Source/c_output.c | 141 ++++++++++++++------------- 3 files changed, 196 insertions(+), 177 deletions(-) diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index fc74b3a..6f562ca 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -461,32 +461,34 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) case RC_SET_OUT_STATE: { + UBYTE Port = pInBuf[1]; //Don't do anything if illegal port specification is made - if (pInBuf[1] >= NO_OF_OUTPUTS && pInBuf[1] != 0xFF) + if (Port >= NO_OF_OUTPUTS && Port != 0xFF) { RCStatus = ERR_RC_ILLEGAL_VAL; break; } //0xFF is protocol defined to mean "all ports". - if (pInBuf[1] == 0xFF) + if (Port == 0xFF) { FirstPort = 0; LastPort = NO_OF_OUTPUTS - 1; } else - FirstPort = LastPort = pInBuf[1]; + FirstPort = LastPort = Port; for (i = FirstPort; i <= LastPort; i++) { - pMapOutPut->Outputs[i].Speed = pInBuf[2]; - pMapOutPut->Outputs[i].Mode = pInBuf[3]; - pMapOutPut->Outputs[i].RegMode = pInBuf[4]; - pMapOutPut->Outputs[i].SyncTurnParameter = pInBuf[5]; - pMapOutPut->Outputs[i].RunState = pInBuf[6]; - memcpy((PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), (PSZ)(&pInBuf[7]), 4); - - pMapOutPut->Outputs[i].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT; + OUTPUT * pOut = &(pMapOutPut->Outputs[i]); + pOut->Speed = pInBuf[2]; + pOut->Mode = pInBuf[3]; + pOut->RegMode = pInBuf[4]; + pOut->SyncTurnParameter = pInBuf[5]; + pOut->RunState = pInBuf[6]; + memcpy((PSZ)(&(pOut->TachoLimit)), (PSZ)(&pInBuf[7]), 4); + + pOut->Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT; } } break; @@ -502,12 +504,13 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) RCStatus = ERR_RC_ILLEGAL_VAL; break; } + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); - pMapInput->Inputs[i].SensorType = pInBuf[2]; - pMapInput->Inputs[i].SensorMode = pInBuf[3]; + pIn->SensorType = pInBuf[2]; + pIn->SensorMode = pInBuf[3]; //Set InvalidData flag automatically since type may have changed - pMapInput->Inputs[i].InvalidData = TRUE; + pIn->InvalidData = TRUE; } break; @@ -525,45 +528,46 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) ResponseLen += 22; break; } + OUTPUT * pOut = &(pMapOutPut->Outputs[i]); //Echo port pOutBuf[ResponseLen] = i; ResponseLen++; //Power - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Speed; + pOutBuf[ResponseLen] = pOut->Speed; ResponseLen++; //Mode - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Mode; + pOutBuf[ResponseLen] = pOut->Mode; ResponseLen++; //RegMode - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RegMode; + pOutBuf[ResponseLen] = pOut->RegMode; ResponseLen++; //TurnRatio - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].SyncTurnParameter; + pOutBuf[ResponseLen] = pOut->SyncTurnParameter; ResponseLen++; //RunState - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RunState; + pOutBuf[ResponseLen] = pOut->RunState; ResponseLen++; //TachoLimit ULONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoLimit)), 4); ResponseLen += 4; //TachoCount SLONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoCnt)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoCnt)), 4); ResponseLen += 4; //BlockTachoCount SLONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].BlockTachoCount)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->BlockTachoCount)), 4); ResponseLen += 4; //RotationCount SLONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].RotationCount)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->RotationCount)), 4); ResponseLen += 4; NXT_ASSERT(ResponseLen == 23); @@ -590,8 +594,10 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) pOutBuf[ResponseLen] = i; ResponseLen++; + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); + //Set "Valid?" boolean - if (pMapInput->Inputs[i].InvalidData) + if (pIn->InvalidData) pOutBuf[ResponseLen] = FALSE; else pOutBuf[ResponseLen] = TRUE; @@ -603,24 +609,24 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) pOutBuf[ResponseLen] = FALSE; ResponseLen++; - pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorType; + pOutBuf[ResponseLen] = pIn->SensorType; ResponseLen++; - pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorMode; + pOutBuf[ResponseLen] = pIn->SensorMode; ResponseLen++; //Set Raw, Normalized, and Scaled values - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].ADRaw)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->ADRaw)), 2); ResponseLen += 2; - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2); ResponseLen += 2; - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorValue)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorValue)), 2); ResponseLen += 2; //!!! Return normalized raw value in place of calibrated value for now -- see comment above - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2); ResponseLen += 2; NXT_ASSERT(ResponseLen == 14); @@ -683,14 +689,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) //pInBuf[2] is a selector //FALSE: Position relative to start of last program //TRUE: Position relative to start of last motor control block - if (pInBuf[2] == FALSE) - { - pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_ROTATION_COUNT; - } - else - { - pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_BLOCK_COUNT; - } + pMapOutPut->Outputs[i].Flags |= (pInBuf[2] ? UPDATE_RESET_BLOCK_COUNT : UPDATE_RESET_ROTATION_COUNT); } break; @@ -1092,32 +1091,34 @@ void cCmdInit(void* pHeader) //Initialize IO_PTRS_OUT for (i = 0; i < NO_OF_OUTPUTS; i++) { - IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Flags); - IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Mode); - IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Speed); - IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].ActualSpeed); - IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoCnt); - IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoLimit); - IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RunState); - IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].SyncTurnParameter); - IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegMode); - IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Overloaded); - IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegPParameter); - IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegIParameter); - IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegDParameter); - IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].BlockTachoCount); - IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RotationCount); + OUTPUT * pOut = &(pMapOutPut->Outputs[i]); + IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pOut->Flags); + IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pOut->Mode); + IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->Speed); + IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->ActualSpeed); + IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->TachoCnt); + IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pOut->TachoLimit); + IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pOut->RunState); + IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pOut->SyncTurnParameter); + IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pOut->RegMode); + IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pOut->Overloaded); + IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegPParameter); + IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegIParameter); + 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); } //Initialize IO_PTRS_IN for (i = 0; i < NO_OF_INPUTS; i++) { - IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorType); - IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorMode); - IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].ADRaw); - IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorRaw); - IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorValue); - IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].InvalidData); + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); + IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pIn->SensorType); + IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pIn->SensorMode); + IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pIn->ADRaw); + IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pIn->SensorRaw); + IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pIn->SensorValue); + IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pIn->InvalidData); } //Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd) @@ -1962,29 +1963,31 @@ void cCmdResetDevices(void) for (i = 0; i < NO_OF_INPUTS; i++) { + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); //Clear type and mode to defaults - pMapInput->Inputs[i].SensorType = NO_SENSOR; - pMapInput->Inputs[i].SensorMode = RAWMODE; + pIn->SensorType = NO_SENSOR; + pIn->SensorMode = RAWMODE; //Reset input values to 0 prior to running (clear things like stale rotation counts) - pMapInput->Inputs[i].ADRaw = 0; - pMapInput->Inputs[i].SensorRaw = 0; - pMapInput->Inputs[i].SensorValue = 0; + pIn->ADRaw = 0; + pIn->SensorRaw = 0; + pIn->SensorValue = 0; //Assert invalid data flag so future code is aware of these changes - pMapInput->Inputs[i].InvalidData = TRUE; + pIn->InvalidData = TRUE; } for (i = 0; i < NO_OF_OUTPUTS; i++) { //Coast and reset all motor parameters - pMapOutPut->Outputs[i].Mode = 0; - pMapOutPut->Outputs[i].RegMode = REGULATION_MODE_IDLE; - pMapOutPut->Outputs[i].RunState = MOTOR_RUN_STATE_IDLE; - pMapOutPut->Outputs[i].Speed = 0; - pMapOutPut->Outputs[i].TachoLimit = 0; - pMapOutPut->Outputs[i].SyncTurnParameter = 0; - pMapOutPut->Outputs[i].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT; + OUTPUT * pOut = &(pMapOutPut->Outputs[i]); + pOut->Mode = 0; + pOut->RegMode = REGULATION_MODE_IDLE; + pOut->RunState = MOTOR_RUN_STATE_IDLE; + pOut->Speed = 0; + pOut->TachoLimit = 0; + pOut->SyncTurnParameter = 0; + pOut->Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT; } //Lowspeed init, INSERT CODE !!! @@ -3243,25 +3246,27 @@ NXT_STATUS cCmdDatalogRead(UBYTE * pBuffer, UWORD Length, UBYTE Remove) NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArray, UWORD * NormalizedArray, SWORD * ScaledArray, UBYTE * InvalidData) { - ULONG i; + ULONG i; //Make sure Port is valid for Color Sensor - if (!(pMapInput->Inputs[Port].SensorType == COLORFULL || pMapInput->Inputs[Port].SensorType == COLORRED - || pMapInput->Inputs[Port].SensorType == COLORGREEN || pMapInput->Inputs[Port].SensorType == COLORBLUE - || pMapInput->Inputs[Port].SensorType == COLORNONE)) + INPUTSTRUCT * pIn = &(pMapInput->Inputs[Port]); + UBYTE sType = pIn->SensorType; + if (!(sType == COLORFULL || sType == COLORRED || sType == COLORGREEN || + sType == COLORBLUE || sType == COLORNONE)) { return (ERR_COMM_CHAN_NOT_READY); //TODO - is this the right error? } //Copy Detected Color - *SensorValue = pMapInput->Inputs[Port].SensorValue; + *SensorValue = pIn->SensorValue; //Copy all raw, normalized and scaled data from I/O Map for (i=0; iColors[Port].ADRaw[i]; - NormalizedArray[i] = pMapInput->Colors[Port].SensorRaw[i]; - ScaledArray[i] = pMapInput->Colors[Port].SensorValue[i]; + COLORSTRUCT * pColor = &(pMapInput->Colors[Port]); + RawArray[i] = pColor->ADRaw[i]; + NormalizedArray[i] = pColor->SensorRaw[i]; + ScaledArray[i] = pColor->SensorValue[i]; } //Copy the Invalid Data Flag - *InvalidData = pMapInput->Inputs[Port].InvalidData; + *InvalidData = pIn->InvalidData; return NO_ERR; @@ -6326,10 +6331,12 @@ NXT_STATUS cCmdLSCheckStatus(UBYTE Port) return (ERR_COMM_CHAN_INVALID); } + INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]); + //If port is not configured properly ahead of time, report that error //!!! This seems like the right policy, but may restrict otherwise valid read operations... - if (!(pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED) - || !(pMapInput->Inputs[Port].InvalidData == FALSE)) + if (!(pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED) + || !(pInput->InvalidData == FALSE)) { return (ERR_COMM_CHAN_NOT_READY); } @@ -6346,13 +6353,15 @@ UBYTE cCmdLSCalcBytesReady(UBYTE Port) //Expect callers to validate Port, but short circuit here to be safe. if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) return 0; + + LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]); //Normally, bytes available is a simple difference. - Tmp = pMapLowSpeed->InBuf[Port].InPtr - pMapLowSpeed->InBuf[Port].OutPtr; + Tmp = pInBuf->InPtr - pInBuf->OutPtr; //If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage... if (Tmp < 0) - Tmp = (pMapLowSpeed->InBuf[Port].InPtr + (SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr)); + Tmp = (pInBuf->InPtr + (SIZE_OF_LSBUF - pInBuf->OutPtr)); return (UBYTE)(Tmp); } @@ -6371,20 +6380,24 @@ NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseL return (ERR_INVALID_SIZE); } + INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]); + UBYTE * pChState = &(pMapLowSpeed->ChannelState[Port]); + LSBUF * pOutBuf = &(pMapLowSpeed->OutBuf[Port]); + //Only start writing process if port is properly configured and c_lowspeed module is ready - if ((pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED) - && (pMapInput->Inputs[Port].InvalidData == FALSE) - && (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_ERROR)) + if ((pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED) + && (pInput->InvalidData == FALSE) + && (*pChState == LOWSPEED_IDLE) || (*pChState == LOWSPEED_ERROR)) { - pMapLowSpeed->OutBuf[Port].InPtr = 0; - pMapLowSpeed->OutBuf[Port].OutPtr = 0; + pOutBuf->InPtr = 0; + pOutBuf->OutPtr = 0; - memcpy(pMapLowSpeed->OutBuf[Port].Buf, pBuf, BufLength); - pMapLowSpeed->OutBuf[Port].InPtr = (UBYTE)BufLength; + memcpy(pOutBuf->Buf, pBuf, BufLength); + pOutBuf->InPtr = (UBYTE)BufLength; pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength; - pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT; + *pChState = LOWSPEED_INIT; pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port); return (NO_ERR); @@ -6422,18 +6435,20 @@ NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf) BytesToRead = BufLength; + LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]); + //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning - if (pMapLowSpeed->InBuf[Port].OutPtr + BytesToRead >= SIZE_OF_LSBUF) + if (pInBuf->OutPtr + BytesToRead >= SIZE_OF_LSBUF) { - BytesToRead = SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr; - memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead); - pMapLowSpeed->InBuf[Port].OutPtr = 0; + BytesToRead = SIZE_OF_LSBUF - pInBuf->OutPtr; + memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead); + pInBuf->OutPtr = 0; pBuf += BytesToRead; BytesToRead = BufLength - BytesToRead; } - memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead); - pMapLowSpeed->InBuf[Port].OutPtr += BytesToRead; + memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead); + pInBuf->OutPtr += BytesToRead; return (NO_ERR); } diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index 47ca7c0..ffd129b 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -226,8 +226,11 @@ void cInputCtrl(void) for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++) { + UBYTE sType = IOMapInput.Inputs[Tmp].SensorType; + UBYTE *pType = &IOMapInput.Inputs[Tmp].SensorType; + UBYTE oldType = VarsInput.OldSensorType[Tmp]; - if ((IOMapInput.Inputs[Tmp].SensorType) != (VarsInput.OldSensorType[Tmp])) + if (sType != oldType) { /* Clear all variables for this sensor */ @@ -239,9 +242,10 @@ void cInputCtrl(void) memset(&(VarsInput.VarsColor[Tmp]),0 ,sizeof(VarsInput.VarsColor[Tmp])); /* Setup the pins for the new sensortype */ - cInputSetupType(Tmp, &(IOMapInput.Inputs[Tmp].SensorType), VarsInput.OldSensorType[Tmp]); + cInputSetupType(Tmp, pType, oldType); + sType = *pType; IOMapInput.Inputs[Tmp].InvalidData = INVALID_DATA; - VarsInput.OldSensorType[Tmp] = IOMapInput.Inputs[Tmp].SensorType; + VarsInput.OldSensorType[Tmp] = sType; } else { @@ -250,12 +254,9 @@ void cInputCtrl(void) /* A type change has been carried out earlier - waiting for valid data */ /* The color sensor requires special startup sequence with communication */ - if (((IOMapInput.Inputs[Tmp].SensorType) == COLORFULL) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLORRED) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLORGREEN)|| - ((IOMapInput.Inputs[Tmp].SensorType) == COLORBLUE) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLOREXIT) || - ((IOMapInput.Inputs[Tmp].SensorType) == COLORNONE)) + if ((sType == COLORFULL) || (sType == COLORRED) || + (sType == COLORGREEN)|| (sType == COLORBLUE) || + (sType == COLOREXIT) || (sType == COLORNONE)) { cInputCalcSensorValues(Tmp); } 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 862733114f99059b8503a0eece085c801d4d76f4 Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 00:44:59 +0100 Subject: factorize code in input module Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_input.c | 219 +++++++----------------------------------- 1 file changed, 35 insertions(+), 184 deletions(-) diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index ffd129b..e118291 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -288,158 +288,58 @@ void cInputCtrl(void) void cInputCalcSensorValues(UBYTE No) { + UBYTE sType = IOMapInput.Inputs[No].SensorType; - switch(IOMapInput.Inputs[No].SensorType) + switch(sType) { case SWITCH: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - case TEMPERATURE: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - if (InputVal < 290) - { - InputVal = 290; - } - else - { - if (InputVal > 928) - { - InputVal = 928; - } - } - InputVal = TempConvTable[(InputVal) - 197]; - InputVal = InputVal + 200; - InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - case REFLECTION: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - case ANGLE: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } - break; - - /* Dual case intended */ case LIGHT_ACTIVE: case LIGHT_INACTIVE: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - - } - break; - - /* Dual case intended */ case SOUND_DB: case SOUND_DBA: - { - UWORD InputVal; - - dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE); - cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), - &(VarsInput.InputDebounce[No]), - &(VarsInput.SampleCnt[No]), - &(VarsInput.LastAngle[No]), - &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - - } - break; - case CUSTOM: { UWORD InputVal; - /* Setup and read digital IO */ - cInputSetupCustomSensor(No); - dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn)); - dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + if (sType == CUSTOM) { + /* Setup and read digital IO */ + cInputSetupCustomSensor(No); + dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + } dInputGetRawAd(&InputVal, No); IOMapInput.Inputs[No].ADRaw = InputVal; - cInputCalcFullScale(&InputVal, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE); + + if (sType == REFLECTION) + { + cInputCalcFullScale(&InputVal, REFLECTIONSENSORMIN, REFLECTIONSENSORPCTDYN, TRUE); + } + else if (sType == TEMPERATURE) + { + if (InputVal < 290) + InputVal = 290; + else if (InputVal > 928) + InputVal = 928; + InputVal = TempConvTable[(InputVal) - 197]; + InputVal = InputVal + 200; + InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); + } + else if (sType == LIGHT_ACTIVE || sType == LIGHT_INACTIVE) + { + cInputCalcFullScale(&InputVal, NEWLIGHTSENSORMIN, NEWLIGHTSENSORPCTDYN, TRUE); + } + else if (sType == SOUND_DB || sType == SOUND_DBA) + { + cInputCalcFullScale(&InputVal, NEWSOUNDSENSORMIN, NEWSOUNDSENSORPCTDYN, TRUE); + } + else if (sType == CUSTOM) + { + cInputCalcFullScale(&InputVal, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE); + } cInputCalcSensorValue( InputVal, &(IOMapInput.Inputs[No].SensorRaw), &(IOMapInput.Inputs[No].SensorValue), @@ -450,7 +350,6 @@ void cInputCalcSensorValues(UBYTE No) &(VarsInput.EdgeCnt[No]), ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); - } break; @@ -1136,13 +1035,6 @@ void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType) break; case REFLECTION: - { - dInputSetActive(Port); - dInputClearDigi0(Port); - dInputClearDigi1(Port); - } - break; - case ANGLE: { dInputSetActive(Port); @@ -1216,50 +1108,9 @@ void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType) break; case COLORFULL: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - - } - break; - case COLORRED: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - } - break; - case COLORGREEN: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - } - break; - case COLORBLUE: - { - VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; - dInputSetInactive(Port); - dInputSetDigi0(Port); - dInputSetDirInDigi1(Port); - IOMapInput.Colors[Port].CalibrationState = SENSORCAL; - VarsInput.VarsColor[Port].ColorInitState = 0; - } - break; - case COLORNONE: { VarsInput.InvalidTimer[Port] = INVALID_RELOAD_COLOR; -- cgit v1.2.3 From 6de19ef892fa6a7e5a4d47c71654e0067632d93d Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 01:37:47 +0100 Subject: remove duplicated code in lowspeed module Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/d_lowspeed.r | 444 +++++---------------------------------- 1 file changed, 51 insertions(+), 393 deletions(-) diff --git a/AT91SAM7S256/Source/d_lowspeed.r b/AT91SAM7S256/Source/d_lowspeed.r index 4b3b8ba..034e76f 100644 --- a/AT91SAM7S256/Source/d_lowspeed.r +++ b/AT91SAM7S256/Source/d_lowspeed.r @@ -141,282 +141,35 @@ ULONG CLK_PINS[4] = {CHANNEL_ONE_CLK, CHANNEL_TWO_CLK, CHANNEL_THREE_CLK, CHANNE #define SETDebugOutputLow *AT91C_PIOA_CODR = 0x20000000 -#define SETClkComOneHigh *AT91C_PIOA_SODR = CHANNEL_ONE_CLK - -#define SETClkComOneLow *AT91C_PIOA_CODR = CHANNEL_ONE_CLK - -#define GetClkComOnePinLevel *AT91C_PIOA_PDSR & CHANNEL_ONE_CLK - -#define SETClkComTwoHigh *AT91C_PIOA_SODR = CHANNEL_TWO_CLK - -#define SETClkComTwoLow *AT91C_PIOA_CODR = CHANNEL_TWO_CLK - -#define GetClkComTwoPinLevel *AT91C_PIOA_PDSR & CHANNEL_TWO_CLK - -#define SETClkComThreeHigh *AT91C_PIOA_SODR = CHANNEL_THREE_CLK - -#define SETClkComThreeLow *AT91C_PIOA_CODR = CHANNEL_THREE_CLK - -#define GetClkComThreePinLevel *AT91C_PIOA_PDSR & CHANNEL_THREE_CLK - -#define SETClkComFourHigh *AT91C_PIOA_SODR = CHANNEL_FOUR_CLK - -#define SETClkComFourLow *AT91C_PIOA_CODR = CHANNEL_FOUR_CLK - -#define GetClkComFourPinLevel *AT91C_PIOA_PDSR & CHANNEL_FOUR_CLK - - -#define SETDataComOneHigh *AT91C_PIOA_SODR = CHANNEL_ONE_DATA - -#define SETDataComOneLow *AT91C_PIOA_CODR = CHANNEL_ONE_DATA - -#define GetDataComOnePinLevel *AT91C_PIOA_PDSR & CHANNEL_ONE_DATA - -#define GETDataComOnePinDirection *AT91C_PIOA_OSR & CHANNEL_ONE_DATA - -#define SETDataComTwoHigh *AT91C_PIOA_SODR = CHANNEL_TWO_DATA - -#define SETDataComTwoLow *AT91C_PIOA_CODR = CHANNEL_TWO_DATA - -#define GetDataComTwoPinLevel *AT91C_PIOA_PDSR & CHANNEL_TWO_DATA - -#define GETDataComTwoPinDirection *AT91C_PIOA_OSR & CHANNEL_TWO_DATA - -#define SETDataComThreeHigh *AT91C_PIOA_SODR = CHANNEL_THREE_DATA - -#define SETDataComThreeLow *AT91C_PIOA_CODR = CHANNEL_THREE_DATA - -#define GetDataComThreePinLevel *AT91C_PIOA_PDSR & CHANNEL_THREE_DATA - -#define GETDataComThreePinDirection *AT91C_PIOA_OSR & CHANNEL_THREE_DATA - -#define SETDataComFourHigh *AT91C_PIOA_SODR = CHANNEL_FOUR_DATA - -#define SETDataComFourLow *AT91C_PIOA_CODR = CHANNEL_FOUR_DATA - -#define GetDataComFourPinLevel *AT91C_PIOA_PDSR & CHANNEL_FOUR_DATA - -#define GETDataComFourPinDirection *AT91C_PIOA_OSR & CHANNEL_FOUR_DATA - -#define SETDataComOneToInput *AT91C_PIOA_ODR = CHANNEL_ONE_DATA; - -#define SETDataComOneToOutput *AT91C_PIOA_OER = CHANNEL_ONE_DATA; - -#define SETDataComTwoToInput *AT91C_PIOA_ODR = CHANNEL_TWO_DATA; - -#define SETDataComTwoToOutput *AT91C_PIOA_OER = CHANNEL_TWO_DATA; - -#define SETDataComThreeToInput *AT91C_PIOA_ODR = CHANNEL_THREE_DATA; - -#define SETDataComThreeToOutput *AT91C_PIOA_OER = CHANNEL_THREE_DATA; - -#define SETDataComFourToInput *AT91C_PIOA_ODR = CHANNEL_FOUR_DATA; - -#define SETDataComFourToOutput *AT91C_PIOA_OER = CHANNEL_FOUR_DATA; - -#define DISABLEPullupDataComOne *AT91C_PIOA_PPUDR = CHANNEL_ONE_DATA; - -#define DISABLEPullupClkComOne *AT91C_PIOA_PPUDR = CHANNEL_ONE_CLK; - -#define DISABLEPullupDataComTwo *AT91C_PIOA_PPUDR = CHANNEL_TWO_DATA; - -#define DISABLEPullupClkComTwo *AT91C_PIOA_PPUDR = CHANNEL_TWO_CLK; - -#define DISABLEPullupDataComThree *AT91C_PIOA_PPUDR = CHANNEL_THREE_DATA; - -#define DISABLEPullupClkComThree *AT91C_PIOA_PPUDR = CHANNEL_THREE_CLK; - -#define DISABLEPullupDataComFour *AT91C_PIOA_PPUDR = CHANNEL_FOUR_DATA; - -#define DISABLEPullupClkComFour *AT91C_PIOA_PPUDR = CHANNEL_FOUR_CLK; - -#define ENABLEPullupDataComOne *AT91C_PIOA_PPUER = CHANNEL_ONE_DATA; - -#define ENABLEPullupClkComOne *AT91C_PIOA_PPUER = CHANNEL_ONE_CLK; - -#define ENABLEPullupDataComTwo *AT91C_PIOA_PPUER = CHANNEL_TWO_DATA; +#define SETClkLow(ChannelNr) {\ + *AT91C_PIOA_CODR = CLK_PINS[ChannelNr];\ + LowSpeedData[ChannelNr].ClkStatus = 0;\ +} -#define ENABLEPullupClkComTwo *AT91C_PIOA_PPUER = CHANNEL_TWO_CLK; +#define SETClkHigh(ChannelNr) {\ + *AT91C_PIOA_SODR = CLK_PINS[ChannelNr];\ + LowSpeedData[ChannelNr].ClkStatus = 1;\ +} -#define ENABLEPullupDataComThree *AT91C_PIOA_PPUER = CHANNEL_THREE_DATA; +#define SETDataLow(ChannelNr) {\ + *AT91C_PIOA_CODR = DATA_PINS[ChannelNr];\ +} -#define ENABLEPullupClkComThree *AT91C_PIOA_PPUER = CHANNEL_THREE_CLK; +#define SETDataHigh(ChannelNr) {\ + *AT91C_PIOA_SODR = DATA_PINS[ChannelNr];\ +} -#define ENABLEPullupDataComFour *AT91C_PIOA_PPUER = CHANNEL_FOUR_DATA; +#define SETDataToInput(ChannelNr) {\ + *AT91C_PIOA_ODR = DATA_PINS[ChannelNr];\ +} -#define ENABLEPullupClkComFour *AT91C_PIOA_PPUER = CHANNEL_FOUR_CLK; -#define SETClkLow(ChannelNr) {\ - if (ChannelNr == 0)\ - {\ - SETClkComOneLow;\ - }\ - else\ - {\ - if (ChannelNr == 1)\ - {\ - SETClkComTwoLow;\ - }\ - else\ - {\ - if (ChannelNr == 2)\ - {\ - SETClkComThreeLow;\ - }\ - else\ - {\ - if (ChannelNr == 3)\ - {\ - SETClkComFourLow;\ - }\ - }\ - }\ - }\ - LowSpeedData[ChannelNr].ClkStatus = 0;\ - } - -#define SETClkHigh(ChannelNr) {\ - if (ChannelNr == 0)\ - {\ - SETClkComOneHigh;\ - }\ - else\ - {\ - if (ChannelNr == 1)\ - {\ - SETClkComTwoHigh;\ - }\ - else\ - {\ - if (ChannelNr == 2)\ - {\ - SETClkComThreeHigh;\ - }\ - else\ - {\ - if (ChannelNr == 3)\ - {\ - SETClkComFourHigh;\ - }\ - }\ - }\ - }\ - LowSpeedData[ChannelNr].ClkStatus = 1;\ - } - -#define SETDataLow(ChannelNr) {\ - if (ChannelNr == 0)\ - {\ - SETDataComOneLow;\ - }\ - else\ - {\ - if (ChannelNr == 1)\ - {\ - SETDataComTwoLow;\ - }\ - else\ - {\ - if (ChannelNr == 2)\ - {\ - SETDataComThreeLow;\ - }\ - else\ - {\ - if (ChannelNr == 3)\ - {\ - SETDataComFourLow;\ - }\ - }\ - }\ - }\ - } - -#define SETDataHigh(ChannelNr) {\ - if (ChannelNr == 0)\ - {\ - SETDataComOneHigh;\ - }\ - else\ - {\ - if (ChannelNr == 1)\ - {\ - SETDataComTwoHigh;\ - }\ - else\ - {\ - if (ChannelNr == 2)\ - {\ - SETDataComThreeHigh;\ - }\ - else\ - {\ - if (ChannelNr == 3)\ - {\ - SETDataComFourHigh;\ - }\ - }\ - }\ - }\ - } - -#define SETDataToInput(ChannelNr) {\ - if (ChannelNr == 0)\ - {\ - SETDataComOneToInput;\ - }\ - else\ - {\ - if (ChannelNr == 1)\ - {\ - SETDataComTwoToInput;\ - }\ - else\ - {\ - if (ChannelNr == 2)\ - {\ - SETDataComThreeToInput;\ - }\ - else\ - {\ - if (ChannelNr == 3)\ - {\ - SETDataComFourToInput;\ - }\ - }\ - }\ - }\ - } - - -#define SETDataToOutput(ChannelNr) {\ - if (ChannelNr == 0)\ - {\ - SETDataComOneToOutput;\ - }\ - else\ - {\ - if (ChannelNr == 1)\ - {\ - SETDataComTwoToOutput;\ - }\ - else\ - {\ - if (ChannelNr == 2)\ - {\ - SETDataComThreeToOutput;\ - }\ - else\ - {\ - if (ChannelNr == 3)\ - {\ - SETDataComFourToOutput;\ - }\ - }\ - }\ - }\ - } +#define SETDataToOutput(ChannelNr) {\ + *AT91C_PIOA_OER = DATA_PINS[ChannelNr];\ +} +#define GetClkPinLevel(ChannelNr) (*AT91C_PIOA_PDSR & CLK_PINS[ChannelNr]) +#define GetDataPinLevel(ChannelNr) (*AT91C_PIOA_PDSR & DATA_PINS[ChannelNr]) #define ENABLEPWMTimerForLowCom {\ *AT91C_PWMC_ENA = AT91C_PWMC_CHID0; /* Enable PWM output channel 0 */\ @@ -743,132 +496,37 @@ __ramfunc void LowSpeedPwmIrqHandler(void) } } - -#define ENABLETxPins(ChannelNumber) {\ - if (ChannelNumber == LOWSPEED_CHANNEL1)\ - {\ - *AT91C_PIOA_PER = CHANNEL_ONE_CLK | CHANNEL_ONE_DATA; /* Enable PIO on PA20 & PA28 */\ - *AT91C_PIOA_PPUDR = CHANNEL_ONE_CLK | CHANNEL_ONE_DATA; /* Disable Pull-up resistor */\ - *AT91C_PIOA_ODR = CHANNEL_ONE_CLK | CHANNEL_ONE_DATA; /* PA20 & PA28 set to Input */\ - }\ - if (ChannelNumber == LOWSPEED_CHANNEL2)\ - {\ - *AT91C_PIOA_PER = CHANNEL_TWO_CLK | CHANNEL_TWO_DATA; /* Enable PIO on PA20 & PA28 */\ - *AT91C_PIOA_PPUDR = CHANNEL_TWO_CLK | CHANNEL_TWO_DATA; /* Disable Pull-up resistor */\ - *AT91C_PIOA_ODR = CHANNEL_TWO_CLK | CHANNEL_TWO_DATA; /* PA20 & PA28 set to Input */\ - }\ - if (ChannelNumber == LOWSPEED_CHANNEL3)\ - {\ - *AT91C_PIOA_PER = CHANNEL_THREE_CLK | CHANNEL_THREE_DATA; /* */\ - *AT91C_PIOA_PPUDR = CHANNEL_THREE_CLK | CHANNEL_THREE_DATA; /* */\ - *AT91C_PIOA_ODR = CHANNEL_THREE_CLK | CHANNEL_THREE_DATA; /* */\ - }\ - if (ChannelNumber == LOWSPEED_CHANNEL4)\ - {\ - *AT91C_PIOA_PER = CHANNEL_FOUR_CLK | CHANNEL_FOUR_DATA; /* */\ - *AT91C_PIOA_PPUDR = CHANNEL_FOUR_CLK | CHANNEL_FOUR_DATA; /* */\ - *AT91C_PIOA_ODR = CHANNEL_FOUR_CLK | CHANNEL_FOUR_DATA; /* */\ - }\ - } +#define ENABLETxPins(ChannelNumber) {\ + ULONG Tmp = CLK_PINS[ChannelNumber] | DATA_PINS[ChannelNumber];\ + *AT91C_PIOA_PER = Tmp; /* Enable PIO */\ + *AT91C_PIOA_PPUDR = Tmp; /* Disable Pull-up resistor */\ + *AT91C_PIOA_ODR = Tmp; /* PIO set to Input */\ +} #define TxData(ChannelNumber, Status, DataOutBuffer, NumberOfByte) {\ - if (ChannelNumber == LOWSPEED_CHANNEL1)\ - {\ - if ((GetDataComOnePinLevel && GetClkComOnePinLevel) && (LowSpeedData[LOWSPEED_CHANNEL1].ChannelState == LOWSPEED_IDLE))\ - {\ - *AT91C_PIOA_PER = CHANNEL_ONE_CLK | CHANNEL_ONE_DATA; /* Enable PIO on PA20 & PA28 */\ - *AT91C_PIOA_OER = CHANNEL_ONE_CLK | CHANNEL_ONE_DATA; /* PA20 & PA28 set to Output */\ - *AT91C_PIOA_PPUDR = CHANNEL_ONE_CLK | CHANNEL_ONE_DATA; /* Disable Pull-up resistor */\ - SETClkComOneHigh;\ - SETDataComOneLow;\ - LowSpeedData[LOWSPEED_CHANNEL1].ClkStatus = 1;\ - LowSpeedData[LOWSPEED_CHANNEL1].pComOutBuffer = DataOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL1].ComDeviceAddress = *LowSpeedData[LOWSPEED_CHANNEL1].pComOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL1].MaskBit = MASK_BIT_8;\ - LowSpeedData[LOWSPEED_CHANNEL1].TxByteCnt = NumberOfByte;\ - LowSpeedData[LOWSPEED_CHANNEL1].TxState = TX_DATA_CLK_HIGH;\ - LowSpeedData[LOWSPEED_CHANNEL1].AckStatus = 0;\ - LowSpeedData[LOWSPEED_CHANNEL1].ChannelState = LOWSPEED_TRANSMITTING;\ - Status = 1;\ - }\ - else\ - {\ - Status = 0;\ - }\ - }\ - if (ChannelNumber == LOWSPEED_CHANNEL2)\ - {\ - if ((GetDataComTwoPinLevel && GetClkComTwoPinLevel) && (LowSpeedData[LOWSPEED_CHANNEL2].ChannelState == LOWSPEED_IDLE))\ - {\ - *AT91C_PIOA_PER = CHANNEL_TWO_CLK | CHANNEL_TWO_DATA; /* Enable PIO on PA20 & PA28 */\ - *AT91C_PIOA_OER = CHANNEL_TWO_CLK | CHANNEL_TWO_DATA; /* PA20 & PA28 set to Output */\ - *AT91C_PIOA_PPUDR = CHANNEL_TWO_CLK | CHANNEL_TWO_DATA; /* Disable Pull-up resistor */\ - SETClkComTwoHigh;\ - SETDataComTwoLow;\ - LowSpeedData[LOWSPEED_CHANNEL2].ClkStatus = 1;\ - LowSpeedData[LOWSPEED_CHANNEL2].pComOutBuffer = DataOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL2].ComDeviceAddress = *LowSpeedData[LOWSPEED_CHANNEL2].pComOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL2].MaskBit = MASK_BIT_8;\ - LowSpeedData[LOWSPEED_CHANNEL2].TxByteCnt = NumberOfByte;\ - LowSpeedData[LOWSPEED_CHANNEL2].TxState = TX_DATA_CLK_HIGH;\ - LowSpeedData[LOWSPEED_CHANNEL2].AckStatus = 0;\ - LowSpeedData[LOWSPEED_CHANNEL2].ChannelState = LOWSPEED_TRANSMITTING;\ - Status = 1;\ - }\ - else\ - {\ - Status = 0;\ - }\ - }\ - if (ChannelNumber == LOWSPEED_CHANNEL3)\ - {\ - if ((GetDataComThreePinLevel && GetClkComThreePinLevel) && (LowSpeedData[LOWSPEED_CHANNEL3].ChannelState == LOWSPEED_IDLE))\ - {\ - *AT91C_PIOA_PER = CHANNEL_THREE_CLK | CHANNEL_THREE_DATA; /* */\ - *AT91C_PIOA_OER = CHANNEL_THREE_CLK | CHANNEL_THREE_DATA; /* */\ - *AT91C_PIOA_PPUDR = CHANNEL_THREE_CLK | CHANNEL_THREE_DATA; /* */\ - SETClkComThreeHigh;\ - SETDataComThreeLow;\ - LowSpeedData[LOWSPEED_CHANNEL3].ClkStatus = 1;\ - LowSpeedData[LOWSPEED_CHANNEL3].pComOutBuffer = DataOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL3].ComDeviceAddress = *LowSpeedData[LOWSPEED_CHANNEL3].pComOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL3].MaskBit = MASK_BIT_8;\ - LowSpeedData[LOWSPEED_CHANNEL3].TxByteCnt = NumberOfByte;\ - LowSpeedData[LOWSPEED_CHANNEL3].TxState = TX_DATA_CLK_HIGH;\ - LowSpeedData[LOWSPEED_CHANNEL3].AckStatus = 0;\ - LowSpeedData[LOWSPEED_CHANNEL3].ChannelState = LOWSPEED_TRANSMITTING;\ - Status = 1;\ - }\ - else\ - {\ - Status = 0;\ - }\ - }\ - if (ChannelNumber == LOWSPEED_CHANNEL4)\ - {\ - if ((GetDataComFourPinLevel && GetClkComFourPinLevel) && (LowSpeedData[LOWSPEED_CHANNEL4].ChannelState == LOWSPEED_IDLE))\ - {\ - *AT91C_PIOA_PER = CHANNEL_FOUR_CLK | CHANNEL_FOUR_DATA; /* */\ - *AT91C_PIOA_OER = CHANNEL_FOUR_CLK | CHANNEL_FOUR_DATA; /* */\ - *AT91C_PIOA_PPUDR = CHANNEL_FOUR_CLK | CHANNEL_FOUR_DATA; /* */\ - SETClkComFourHigh;\ - SETDataComFourLow;\ - LowSpeedData[LOWSPEED_CHANNEL4].ClkStatus = 1;\ - LowSpeedData[LOWSPEED_CHANNEL4].pComOutBuffer = DataOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL4].ComDeviceAddress = *LowSpeedData[LOWSPEED_CHANNEL4].pComOutBuffer;\ - LowSpeedData[LOWSPEED_CHANNEL4].MaskBit = MASK_BIT_8;\ - LowSpeedData[LOWSPEED_CHANNEL4].TxByteCnt = NumberOfByte;\ - LowSpeedData[LOWSPEED_CHANNEL4].TxState = TX_DATA_CLK_HIGH;\ - LowSpeedData[LOWSPEED_CHANNEL4].AckStatus = 0;\ - LowSpeedData[LOWSPEED_CHANNEL4].ChannelState = LOWSPEED_TRANSMITTING;\ - Status = 1;\ - }\ - else\ - {\ - Status = 0;\ - }\ - }\ - } + if ((GetDataPinLevel(ChannelNumber) && GetClkPinLevel(ChannelNumber)) && (LowSpeedData[ChannelNumber].ChannelState == LOWSPEED_IDLE))\ + {\ + ULONG Tmp = CLK_PINS[ChannelNumber] | DATA_PINS[ChannelNumber];\ + *AT91C_PIOA_PER = Tmp; /* Enable PIO */\ + *AT91C_PIOA_OER = Tmp; /* POI set to Output */\ + *AT91C_PIOA_PPUDR = Tmp; /* Disable Pull-up resistor */\ + SETClkHigh(ChannelNumber);\ + SETDataLow(ChannelNumber);\ + LowSpeedData[ChannelNumber].ClkStatus = 1;\ + LowSpeedData[ChannelNumber].pComOutBuffer = DataOutBuffer;\ + LowSpeedData[ChannelNumber].ComDeviceAddress = *LowSpeedData[ChannelNumber].pComOutBuffer;\ + LowSpeedData[ChannelNumber].MaskBit = MASK_BIT_8;\ + LowSpeedData[ChannelNumber].TxByteCnt = NumberOfByte;\ + LowSpeedData[ChannelNumber].TxState = TX_DATA_CLK_HIGH;\ + LowSpeedData[ChannelNumber].AckStatus = 0;\ + LowSpeedData[ChannelNumber].ChannelState = LOWSPEED_TRANSMITTING;\ + Status = 1;\ + }\ + else\ + {\ + Status = 0;\ + }\ +} #define RxData(ChannelNumber, DataInBuffer, RxBytes) {\ LowSpeedData[ChannelNumber].pComInBuffer = DataInBuffer;\ -- cgit v1.2.3 From acb06c77dca2ec9ddd476225ce509c2cb5b05dfb Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 22:32:01 +0100 Subject: replace many array indexes with pointer access, remove duplicated code Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/d_output.c | 1106 +++++++++++++++------------------------- AT91SAM7S256/Source/d_output.h | 14 +- 2 files changed, 431 insertions(+), 689 deletions(-) diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index d953b84..63613f9 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -88,23 +88,24 @@ void dOutputInit(void) 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; INSERTMode(Temp, COAST_MOTOR_MODE); - INSERTSpeed(Temp, MotorData[Temp].MotorSetSpeed); + INSERTSpeed(Temp, pMD->MotorSetSpeed); } } @@ -123,43 +124,44 @@ 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 > REGULATION_TIME) { - MotorData[MotorNr].RegulationTimeCount = 0; + pMD->RegulationTimeCount = 0; dOutputRegulateMotor(MotorNr); - MotorData[MotorNr].DeltaCaptureCount = 0; + pMD->DeltaCaptureCount = 0; } } INSERTSpeed(MOTOR_A, MotorData[MOTOR_A].MotorActualSpeed); @@ -180,18 +182,19 @@ 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; } } -void dOutputSetMode(UBYTE Motor, UBYTE Mode) //Set motor mode (break, Float) +void dOutputSetMode(UBYTE MotorNr, UBYTE Mode) //Set motor mode (break, Float) { - INSERTMode(Motor, Mode); + INSERTMode(MotorNr, Mode); } /* Update the regulation state for the motor */ @@ -199,17 +202,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 +234,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,11 +264,12 @@ 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 */ @@ -271,34 +277,35 @@ void dOutputSetPIDParameters(UBYTE Motor, UBYTE NewRegPParameter, UBYTE NewRegIP /* TachoCountToRun is calculated as a signed value */ void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel) { + MOTORDATA * pMD = &(MotorData[MotorNr]); if (BlockTachoCntToTravel == 0) { - MotorData[MotorNr].MotorRunForever = 1; + pMD->MotorRunForever = 1; } else { - MotorData[MotorNr].MotorRunForever = 0; + pMD->MotorRunForever = 0; - 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 +314,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 +385,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 +398,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 +440,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 += 1; + 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 > 100) { - 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 -= 1; + 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 > 100) { - 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 = MOTOR_RUN_STATE_IDLE; } } 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 = MOTOR_RUN_STATE_IDLE; } } - 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 +550,144 @@ 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 == MOTOR_RUN_STATE_IDLE) { 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->MotorSetSpeed > 0) { - if ((MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun)) + if ((pMD->CurrentCaptureCount >= pMD->MotorTachoCountToRun)) { - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; + pMD->MotorRunState = MOTOR_RUN_STATE_IDLE; + pMD->RegulationMode = REGSTATE_IDLE; } } else { - if (MotorData[MotorNr].MotorSetSpeed < 0) + if (pMD->MotorSetSpeed < 0) { - if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun) + if (pMD->CurrentCaptureCount <= pMD->MotorTachoCountToRun) { - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; + pMD->MotorRunState = MOTOR_RUN_STATE_IDLE; + pMD->RegulationMode = REGSTATE_IDLE; } } } @@ -682,18 +695,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 +715,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 +760,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 +790,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 +819,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 +829,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 +858,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 +879,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 @@ -926,9 +945,9 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) } */ - 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 +960,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 +978,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,190 +1025,81 @@ 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) { - if (MotorNr == MOTOR_A) - { - MotorOne = MotorNr; - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B - MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; - MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - 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].RegulationMode = REGSTATE_IDLE; - } - else - { - MotorTwo = MotorOne + 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C - MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; - MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - 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].RegulationMode = REGSTATE_IDLE; - } - else - { - //Only Motor A has Sync setting => Stop normal - MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; - MotorData[MotorNr].MotorActualSpeed = 0; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; - } - } - } - if (MotorNr == MOTOR_B) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B - MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; - MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - 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].RegulationMode = REGSTATE_IDLE; - } - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C - MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; - MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - 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].RegulationMode = REGSTATE_IDLE; - } - else - { - //Only Motor B has Sync settings => Stop normal - MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; - MotorData[MotorNr].MotorActualSpeed = 0; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; - } - } - if (MotorNr == MOTOR_C) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C - MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; - MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - 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].RegulationMode = REGSTATE_IDLE; - } - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C - MotorData[MotorOne].MotorSetSpeed = 0; - MotorData[MotorOne].MotorTargetSpeed = 0; - MotorData[MotorOne].MotorActualSpeed = 0; - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - 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].RegulationMode = REGSTATE_IDLE; - } - else - { - //Only Motor C has Sync settings => Stop normal - MotorData[MotorNr].MotorSetSpeed = 0; - MotorData[MotorNr].MotorTargetSpeed = 0; - MotorData[MotorNr].MotorActualSpeed = 0; - MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorNr].RegulationMode = REGSTATE_IDLE; - } + 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 (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 = MOTOR_RUN_STATE_IDLE; + pOne->RegulationMode = REGSTATE_IDLE; } } @@ -1199,155 +1109,97 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr) { UBYTE MotorOne, MotorTwo; - if (MotorNr == MOTOR_A) - { - MotorOne = MotorNr; - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B - } - else - { - MotorTwo = MotorOne + 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C - } - else - { - //Only Motor A has Sync setting => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } - } - } - if (MotorNr == MOTOR_B) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B, which has already been called when running throught motor A - //MotorOne = 0xFF; - //MotorTwo = 0xFF; - } - else - { - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C - } - else - { - //Only Motor B has Sync settings => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } - } - } - if (MotorNr == MOTOR_C) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C, which has already been called when running throught motor A - //MotorOne = 0xFF; - //MotorTwo = 0xFF; - } - else - { - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C, which has already been called when running throught motor B - //MotorOne = 0xFF; - //MotorTwo = 0xFF; - } - else - { - //Only Motor C has Sync settings => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } + 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)) { - 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[MotorTwo].MotorTargetSpeed >= 0) + if (pTwo->MotorTargetSpeed >= 0) { - if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun)) + if ((SLONG)(pTwo->CurrentCaptureCount >= pTwo->MotorTachoCountToRun)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = MOTOR_RUN_STATE_IDLE; + pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount; - MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun; + pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount; + pOne->MotorTachoCountToRun = pTwo->MotorTachoCountToRun; } } else { - if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun)) + if ((SLONG)(pOne->CurrentCaptureCount <= pOne->MotorTachoCountToRun)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = MOTOR_RUN_STATE_IDLE; + pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount; - MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun; + pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount; + pTwo->MotorTachoCountToRun = pOne->MotorTachoCountToRun; } } } else { - if (MotorData[MotorOne].MotorTargetSpeed >= 0) + if (pOne->MotorTargetSpeed >= 0) { - if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun)) + if ((SLONG)(pOne->CurrentCaptureCount >= pOne->MotorTachoCountToRun)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = MOTOR_RUN_STATE_IDLE; + pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount; - MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun; + pTwo->CurrentCaptureCount = pOne->CurrentCaptureCount; + pTwo->MotorTachoCountToRun = pOne->MotorTachoCountToRun; } } else { - if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun)) + if ((SLONG)(pTwo->CurrentCaptureCount <= pTwo->MotorTachoCountToRun)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = MOTOR_RUN_STATE_IDLE; + pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount; - MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun; + pOne->CurrentCaptureCount = pTwo->CurrentCaptureCount; + pOne->MotorTachoCountToRun = pTwo->MotorTachoCountToRun; } } } } 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 >= pOne->MotorTachoCountToRun) || (pTwo->CurrentCaptureCount >= pTwo->MotorTachoCountToRun)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = MOTOR_RUN_STATE_IDLE; + pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE; } } 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 <= pOne->MotorTachoCountToRun) || (pTwo->CurrentCaptureCount <= pTwo->MotorTachoCountToRun)) { - MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE; - MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE; + pOne->MotorRunState = MOTOR_RUN_STATE_IDLE; + pTwo->MotorRunState = MOTOR_RUN_STATE_IDLE; } } } @@ -1411,87 +1263,33 @@ void dOutputResetSyncMotors(UBYTE MotorNr) { UBYTE MotorOne, MotorTwo; - if (MotorNr == MOTOR_A) - { - MotorOne = MotorNr; - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B - } - else - { - MotorTwo = MotorOne + 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C - } - else - { - //Only Motor A has Sync setting => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } - } - } - if (MotorNr == MOTOR_B) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B - } - else - { - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C - } - else - { - //Only Motor B has Sync settings => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } - } - } - if (MotorNr == MOTOR_C) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C - } - else - { - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C - } - else - { - //Only Motor C has Sync settings => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } + 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[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,113 +1298,57 @@ void dOutputRampDownSynch(UBYTE MotorNr) { UBYTE MotorOne, MotorTwo; - if (MotorNr == MOTOR_A) - { - MotorOne = MotorNr; - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B - } - else - { - MotorTwo = MotorOne + 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C - } - else - { - //Only Motor A has Sync setting => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } - } - } - if (MotorNr == MOTOR_B) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & B, which has already been called when running throught motor A - //MotorOne = 0xFF; - //MotorTwo = 0xFF; - } - else - { - MotorTwo = MotorOne + 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C - } - else - { - //Only Motor B has Sync settings => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } - } - } - if (MotorNr == MOTOR_C) - { - MotorOne = MotorNr; - MotorTwo = MotorOne - 2; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor A & C, which has already been called when running throught motor A - } - else - { - MotorTwo = MotorOne - 1; - if (MotorData[MotorTwo].RegulationMode & REGSTATE_SYNCHRONE) - { - //Synchronise motor B & C,, which has already been called when running throught motor B - } - else - { - //Only Motor C has Sync settings => Stop normal - MotorOne = 0xFF; - MotorTwo = 0xFF; - } + 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)) { - 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++; } } } diff --git a/AT91SAM7S256/Source/d_output.h b/AT91SAM7S256/Source/d_output.h index 7369b34..1b0576a 100644 --- a/AT91SAM7S256/Source/d_output.h +++ b/AT91SAM7S256/Source/d_output.h @@ -64,15 +64,15 @@ void dOutputExit(void); void dOutputCtrl(void); void dOutputGetMotorParameters(UBYTE *CurrentMotorSpeed, SLONG *TachoCount, SLONG *BlockTachoCount, UBYTE *RunState, UBYTE *MotorOverloaded, SLONG *RotationCount); -void dOutputSetMode(UBYTE Motor, UBYTE Mode); +void dOutputSetMode(UBYTE MotorNr, UBYTE Mode); void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE TurnParameter); -void dOutputEnableRegulation(UBYTE Motor, UBYTE RegulationMode); -void dOutputDisableRegulation(UBYTE Motor); -void dOutputSetTachoLimit(UBYTE Motor, ULONG TachoCntToTravel); -void dOutputResetTachoLimit(UBYTE Motor); -void dOutputResetBlockTachoLimit(UBYTE Motor); +void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode); +void dOutputDisableRegulation(UBYTE MotorNr); +void dOutputSetTachoLimit(UBYTE MotorNr, ULONG TachoCntToTravel); +void dOutputResetTachoLimit(UBYTE MotorNr); +void dOutputResetBlockTachoLimit(UBYTE MotorNr); void dOutputResetRotationCaptureCount(UBYTE MotorNr); -void dOutputSetPIDParameters(UBYTE Motor, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter); +void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter); void dOutputRegulateMotor(UBYTE MotorNr); void dOutputCalculateRampUpParameter(UBYTE MotorNr, ULONG NewTachoLimit); -- cgit v1.2.3 From ea4e008921f457d8111ed237f25b710817c6b34c Mon Sep 17 00:00:00 2001 From: John Hansen Date: Fri, 21 Jan 2011 23:38:39 +0100 Subject: remove unused values in temperature conversion table Imported from NXT Enhanced Firmware. --- AT91SAM7S256/Source/c_input.c | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index e118291..667a167 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -69,12 +69,7 @@ enum const SWORD TempConvTable[] = { - 1500, 1460, 1430, 1400, 1380, 1360, 1330, 1310, 1290, 1270, 1250, 1230, 1220, 1200, 1190, 1170, - 1160, 1150, 1140, 1130, 1110, 1100, 1090, 1080, 1070, 1060, 1050, 1040, 1030, 1020, 1010, 1000, - 994, 988, 982, 974, 968, 960, 954, 946, 940, 932, 926, 918, 912, 906, 900, 894, - 890, 884, 878, 874, 868, 864, 858, 854, 848, 844, 838, 832, 828, 822, 816, 812, - 808, 802, 798, 794, 790, 786, 782, 780, 776, 772, 768, 764, 762, 758, 754, 750, - 748, 744, 740, 736, 732, 730, 726, 722, 718, 716, 712, 708, 704, 700, 696, 694, + 700, 696, 694, 690, 688, 684, 682, 678, 674, 672, 668, 666, 662, 660, 656, 654, 650, 648, 644, 642, 640, 638, 634, 632, 630, 628, 624, 622, 620, 616, 614, 612, 610, 608, 604, 602, 600, 598, 596, 592, 590, 588, 586, 584, 582, 580, 578, 576, 574, 572, 570, @@ -114,13 +109,7 @@ const SWORD TempConvTable[] = -98, -98, -100, -102, -104, -106, -106, -108, -110, -112, -114, -114, -116, -118, -120, -120, -122, -124, -126, -128, -130, -130, -132, -134, -136, -138, -140, -142, -144, -146, -146, -148, -150, -152, -154, -156, -158, -160, -162, -164, -166, -166, -168, -170, -172, -174, -176, -178, - -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200, -202, -204, -206, -208, - -210, -212, -214, -216, -218, -220, -224, -226, -228, -230, -232, -234, -236, -238, -242, -246, - -248, -250, -254, -256, -260, -262, -264, -268, -270, -274, -276, -278, -282, -284, -286, -290, - -292, -296, -298, -300, -306, -308, -312, -316, -320, -324, -326, -330, -334, -338, -342, -344, - -348, -354, -358, -362, -366, -370, -376, -380, -384, -388, -394, -398, -404, -410, -416, -420, - -428, -432, -440, -446, -450, -460, -468, -476, -484, -492, -500, -510, -524, -534, -546, -560, - -572, -588, -600, -630, -656, -684, -720, -770 + -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200 }; static IOMAPINPUT IOMapInput; @@ -324,7 +313,7 @@ void cInputCalcSensorValues(UBYTE No) InputVal = 290; else if (InputVal > 928) InputVal = 928; - InputVal = TempConvTable[(InputVal) - 197]; + InputVal = TempConvTable[(InputVal) - 290]; InputVal = InputVal + 200; InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); } -- cgit v1.2.3 From 8aa1d4b314f219f8d5b30eced790efae0f79b77d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 22 Jan 2011 14:35:13 +0100 Subject: replace RCX temperature conversion table with a polynomial --- AT91SAM7S256/Source/c_input.c | 65 ++++++++++++------------------------------- 1 file changed, 18 insertions(+), 47 deletions(-) diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index 667a167..2cb9be1 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -66,52 +66,6 @@ enum CUSTOM_SETUP = 0x80 }; - -const SWORD TempConvTable[] = -{ - 700, 696, 694, - 690, 688, 684, 682, 678, 674, 672, 668, 666, 662, 660, 656, 654, 650, 648, 644, - 642, 640, 638, 634, 632, 630, 628, 624, 622, 620, 616, 614, 612, 610, 608, 604, - 602, 600, 598, 596, 592, 590, 588, 586, 584, 582, 580, 578, 576, 574, 572, 570, - 568, 564, 562, 560, 558, 556, 554, 552, 550, 548, 546, 544, 542, 540, 538, 536, - 534, 532, 530, 528, 526, 524, 522, 520, 518, 516, 514, 512, 510, 508, 508, 506, - 504, 502, 500, 498, 496, 494, 494, 492, 490, 488, 486, 486, 484, 482, 480, 478, - 476, 476, 474, 472, 470, 468, 468, 466, 464, 462, 460, 458, 458, 456, 454, 452, - 450, 448, 448, 446, 444, 442, 442, 440, 438, 436, 436, 434, 432, 432, 430, 428, - 426, 426, 424, 422, 420, 420, 418, 416, 416, 414, 412, 410, 408, 408, 406, 404, - 404, 402, 400, 398, 398, 396, 394, 394, 392, 390, 390, 388, 386, 386, 384, 382, - 382, 380, 378, 378, 376, 374, 374, 372, 370, 370, 368, 366, 366, 364, 362, 362, - 360, 358, 358, 356, 354, 354, 352, 350, 350, 348, 348, 346, 344, 344, 342, 340, - 340, 338, 338, 336, 334, 334, 332, 332, 330, 328, 328, 326, 326, 324, 322, 322, - 320, 320, 318, 316, 316, 314, 314, 312, 310, 310, 308, 308, 306, 304, 304, 302, - 300, 300, 298, 298, 296, 296, 294, 292, 292, 290, 290, 288, 286, 286, 284, 284, - 282, 282, 280, 280, 278, 278, 276, 274, 274, 272, 272, 270, 270, 268, 268, 266, - 264, 264, 262, 262, 260, 260, 258, 258, 256, 254, 254, 252, 252, 250, 250, 248, - 248, 246, 244, 244, 242, 240, 240, 240, 238, 238, 236, 236, 234, 234, 232, 230, - 230, 228, 228, 226, 226, 224, 224, 222, 220, 220, 218, 218, 216, 216, 214, 214, - 212, 212, 210, 210, 208, 208, 206, 204, 204, 202, 202, 200, 200, 198, 198, 196, - 196, 194, 194, 192, 190, 190, 188, 188, 186, 186, 184, 184, 182, 182, 180, 180, - 178, 178, 176, 176, 174, 174, 172, 172, 170, 170, 168, 168, 166, 166, 164, 164, - 162, 162, 160, 160, 158, 156, 156, 154, 154, 152, 152, 150, 150, 148, 148, 146, - 146, 144, 144, 142, 142, 140, 140, 138, 136, 136, 136, 134, 134, 132, 130, 130, - 128, 128, 126, 126, 124, 124, 122, 122, 120, 120, 118, 118, 116, 116, 114, 114, - 112, 110, 110, 108, 108, 106, 106, 104, 104, 102, 102, 100, 100, 98, 98, 96, - 94, 94, 92, 92, 90, 90, 88, 88, 86, 86, 84, 82, 82, 80, 80, 78, - 78, 76, 76, 74, 74, 72, 72, 70, 70, 68, 68, 66, 66, 64, 62, 62, - 60, 60, 58, 56, 56, 54, 54, 52, 52, 50, 50, 48, 48, 46, 46, 44, - 44, 42, 40, 40, 38, 38, 36, 34, 34, 32, 32, 30, 30, 28, 28, 26, - 24, 24, 22, 22, 20, 20, 18, 16, 16, 14, 14, 12, 12, 10, 10, 8, - 6, 6, 4, 2, 2, 0, 0, -2, -4, -4, -6, -6, -8, -10, -10, -12, - -12, -14, -16, - 16, -18, -20, -20, -22, -22, -24, -26, -26, -28, -30, -30, -32, - -34, -34, -36, -36, -38, -40, -40, -42, -42, -44, -46, -46, -48, -50, -50, -52, - -54, -54, -56, -58, -58, -60, -60, -62, -64, -66, -66, -68, -70, -70, -72, -74, - -76, -76, -78, -80, -80, -82, -84, -86, -86, -88, -90, -90, -92, -94, -94, -96, - -98, -98, -100, -102, -104, -106, -106, -108, -110, -112, -114, -114, -116, -118, -120, -120, - -122, -124, -126, -128, -130, -130, -132, -134, -136, -138, -140, -142, -144, -146, -146, -148, - -150, -152, -154, -156, -158, -160, -162, -164, -166, -166, -168, -170, -172, -174, -176, -178, - -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200 -}; - static IOMAPINPUT IOMapInput; static VARSINPUT VarsInput; @@ -139,6 +93,7 @@ void cInputSetupCustomSensor(UBYTE Port); void cInputCalcSensorValues(UBYTE No); UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus); void cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals); +SWORD cInputTempConv(UWORD InputVal); void cInputInit(void* pHeader) { @@ -313,7 +268,7 @@ void cInputCalcSensorValues(UBYTE No) InputVal = 290; else if (InputVal > 928) InputVal = 928; - InputVal = TempConvTable[(InputVal) - 290]; + InputVal = cInputTempConv(InputVal - 290); InputVal = InputVal + 200; InputVal = (UWORD)(((SLONG)InputVal * (SLONG)1023)/(SLONG)900); } @@ -1165,6 +1120,22 @@ void cInputSetupCustomSensor(UBYTE Port) } +SWORD cInputTempConv(UWORD InputVal) +{ + static const long long TempCoeff[] = { -5425ll, 9261399ll, -6686663252ll, + 2573629857807ll, -822478326197838ll, 195856762719738784ll }; + const unsigned int TempCoeffShift = 48; + /* Replace the original table with polynomial. */ + int i; + long long Input = InputVal; + long long Output = TempCoeff[0]; + for (i = 1; i < sizeof TempCoeff / sizeof TempCoeff[0]; i++) + Output = Output * Input + TempCoeff[i]; + /* Round. */ + return Output + (1ll << TempCoeffShift - 1) >> TempCoeffShift; +} + + UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus) { -- cgit v1.2.3 From b24591b90cf855e9cd194e85e8c095af33400818 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 27 Jan 2011 01:02:25 +0100 Subject: use a function for bounds checking in output code --- AT91SAM7S256/Source/d_output.c | 162 +++++++---------------------------------- 1 file changed, 25 insertions(+), 137 deletions(-) diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index 63613f9..96ff607 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -25,7 +25,6 @@ #define MAX_COUNT_TO_RUN 10000000 #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 @@ -36,6 +35,7 @@ #define COAST_MOTOR_MODE 0 void dOutputRampDownSynch(UBYTE MotorNr); +SLONG dOutputBound(SLONG In, SLONG Limit); typedef struct { @@ -754,6 +754,17 @@ void dOutputMotorIdleControl(UBYTE MotorNr) } } +/* 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) { @@ -798,61 +809,26 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) //Overflow control on PositionError if (pMD->RegPParameter != 0) { - if (PositionError > (SWORD)(32000 / pMD->RegPParameter)) - { - PositionError = (SWORD)(32000 / pMD->RegPParameter); - } - if (PositionError < (SWORD)(-(32000 / pMD->RegPParameter))) - { - PositionError = (SWORD)(-(32000 / pMD->RegPParameter)); - } + PositionError = dOutputBound (PositionError, 32000 / pMD->RegPParameter); } else { - if (PositionError > (SWORD)32000) - { - PositionError = (SWORD)32000; - } - if (PositionError < (SWORD)-32000) - { - PositionError = (SWORD)-32000; - } + PositionError = dOutputBound (PositionError, 32000); } PValue = PositionError * (SWORD)(pMD->RegPParameter/REG_CONST_DIV); - if (PValue > (SWORD)REG_MAX_VALUE) - { - PValue = REG_MAX_VALUE; - } - if (PValue <= (SWORD)REG_MIN_VALUE) - { - PValue = REG_MIN_VALUE; - } + PValue = dOutputBound (PValue, REG_MAX_VALUE); DValue = (PositionError - pMD->OldPositionError) * (SWORD)(pMD->RegDParameter/REG_CONST_DIV); pMD->OldPositionError = PositionError; pMD->AccError = (pMD->AccError * 3) + PositionError; pMD->AccError = pMD->AccError / 4; + pMD->AccError = dOutputBound (pMD->AccError, 800); - if (pMD->AccError > (SWORD)800) - { - pMD->AccError = 800; - } - if (pMD->AccError <= (SWORD)-800) - { - pMD->AccError = -800; - } IValue = pMD->AccError * (SWORD)(pMD->RegIParameter/REG_CONST_DIV); + IValue = dOutputBound (IValue, REG_MAX_VALUE); - if (IValue > (SWORD)REG_MAX_VALUE) - { - IValue = REG_MAX_VALUE; - } - if (IValue <= (SWORD)REG_MIN_VALUE) - { - IValue = REG_MIN_VALUE; - } TotalRegValue = (SWORD)((PValue + IValue + DValue)/2); if (TotalRegValue > MAXIMUM_SPEED_FW) @@ -924,26 +900,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) //SyncTurnParameter should ophold difference between the two motors. 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; - } - if ((SWORD)SyncData.SyncTachoDif < -500) - { - SyncData.SyncTachoDif = -500; - } - */ + SyncData.SyncTachoDif = dOutputBound (SyncData.SyncTachoDif, 500); PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV); @@ -951,109 +908,40 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif; SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif; + SyncData.SyncAccError = dOutputBound (SyncData.SyncAccError, 900); - if (SyncData.SyncAccError > (SWORD)900) - { - SyncData.SyncAccError = 900; - } - if (SyncData.SyncAccError < (SWORD)-900) - { - SyncData.SyncAccError = -900; - } IValue = SyncData.SyncAccError * (SWORD)(pOne->RegIParameter/REG_CONST_DIV); CorrectionValue = (SWORD)((PValue + IValue + DValue)/4); MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue; - - if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW) - { - MotorSpeed = MAXIMUM_SPEED_FW; - } - else - { - if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW) - { - MotorSpeed = MAXIMUM_SPEED_RW; - } - } + MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW); if (pOne->TurnParameter != 0) { if (pOne->MotorTargetSpeed > 0) { - if (MotorSpeed > (SWORD)pOne->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pOne->MotorTargetSpeed; - } - else - { - if (MotorSpeed < (SWORD)-pOne->MotorTargetSpeed) - { - MotorSpeed = -pOne->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, pOne->MotorTargetSpeed); } else { - if (MotorSpeed < (SWORD)pOne->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pOne->MotorTargetSpeed; - } - else - { - if (MotorSpeed > (SWORD)-pOne->MotorTargetSpeed) - { - MotorSpeed = -pOne->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, -pOne->MotorTargetSpeed); } } pOne->MotorActualSpeed = (SBYTE)MotorSpeed; MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue; - - if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW) - { - MotorSpeed = MAXIMUM_SPEED_FW; - } - else - { - if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW) - { - MotorSpeed = MAXIMUM_SPEED_RW; - } - } + MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW); if (pOne->TurnParameter != 0) { if (pTwo->MotorTargetSpeed > 0) { - if (MotorSpeed > (SWORD)pTwo->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pTwo->MotorTargetSpeed; - } - else - { - if (MotorSpeed < (SWORD)-pTwo->MotorTargetSpeed) - { - MotorSpeed = -pTwo->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, pTwo->MotorTargetSpeed); } else { - if (MotorSpeed < (SWORD)pTwo->MotorTargetSpeed) - { - MotorSpeed = (SWORD)pTwo->MotorTargetSpeed; - } - else - { - if (MotorSpeed > (SWORD)-pTwo->MotorTargetSpeed) - { - MotorSpeed = -pTwo->MotorTargetSpeed; - } - } + MotorSpeed = dOutputBound (MotorSpeed, -pTwo->MotorTargetSpeed); } } pTwo->MotorActualSpeed = (SBYTE)MotorSpeed; -- 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(-) 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 From 246cbba0e110c8b19bc6f9950208c8f2d706e4d9 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 27 Jan 2011 20:41:21 +0100 Subject: use SLONG instead of SWORD in output code The ARM is working in 32 bit; there is no gain in working with 16 bit integers which are actually half words on 32 bit systems. --- AT91SAM7S256/Source/d_output.c | 57 ++++++++++++++++++------------------------ 1 file changed, 25 insertions(+), 32 deletions(-) diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c index 96ff607..3608cb7 100644 --- a/AT91SAM7S256/Source/d_output.c +++ b/AT91SAM7S256/Source/d_output.c @@ -794,42 +794,35 @@ void dOutputRegulateMotor(UBYTE MotorNr) /* The regulation form only control one motor at a time */ void dOutputCalculateMotorPosition(UBYTE MotorNr) { - SWORD PositionError; - SWORD PValue; - SWORD IValue; - SWORD DValue; - SWORD TotalRegValue; - SWORD NewSpeedCount = 0; + SLONG PositionError; + SLONG PValue; + SLONG IValue; + SLONG DValue; + SLONG TotalRegValue; + SLONG NewSpeedCount = 0; MOTORDATA * pMD = &(MotorData[MotorNr]); NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR); - PositionError = (SWORD)(pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount; + PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount; //Overflow control on PositionError - if (pMD->RegPParameter != 0) - { - PositionError = dOutputBound (PositionError, 32000 / pMD->RegPParameter); - } - else - { - PositionError = dOutputBound (PositionError, 32000); - } + PositionError = dOutputBound (PositionError, 32000); - PValue = PositionError * (SWORD)(pMD->RegPParameter/REG_CONST_DIV); + PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV); PValue = dOutputBound (PValue, REG_MAX_VALUE); - DValue = (PositionError - pMD->OldPositionError) * (SWORD)(pMD->RegDParameter/REG_CONST_DIV); - pMD->OldPositionError = PositionError; + DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV); + pMD->OldPositionError = (SWORD)PositionError; - pMD->AccError = (pMD->AccError * 3) + PositionError; + pMD->AccError = (pMD->AccError * 3) + (SWORD)PositionError; pMD->AccError = pMD->AccError / 4; pMD->AccError = dOutputBound (pMD->AccError, 800); - IValue = pMD->AccError * (SWORD)(pMD->RegIParameter/REG_CONST_DIV); + IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV); IValue = dOutputBound (IValue, REG_MAX_VALUE); - TotalRegValue = (SWORD)((PValue + IValue + DValue)/2); + TotalRegValue = (PValue + IValue + DValue) / 2; if (TotalRegValue > MAXIMUM_SPEED_FW) { @@ -849,11 +842,11 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr) void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) { SLONG TempTurnParameter; - SWORD PValue; - SWORD IValue; - SWORD DValue; - SWORD CorrectionValue; - SWORD MotorSpeed; + SLONG PValue; + SLONG IValue; + SLONG DValue; + SLONG CorrectionValue; + SLONG MotorSpeed; MOTORDATA * pOne = &(MotorData[MotorOne]); MOTORDATA * pTwo = &(MotorData[MotorTwo]); @@ -902,19 +895,19 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) SyncData.SyncTachoDif += SyncData.SyncTurnParameter; SyncData.SyncTachoDif = dOutputBound (SyncData.SyncTachoDif, 500); - PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV); + PValue = SyncData.SyncTachoDif * (pOne->RegPParameter/REG_CONST_DIV); - DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(pOne->RegDParameter/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 * (SWORD)(pOne->RegIParameter/REG_CONST_DIV); + IValue = SyncData.SyncAccError * (pOne->RegIParameter/REG_CONST_DIV); - CorrectionValue = (SWORD)((PValue + IValue + DValue)/4); + CorrectionValue = (PValue + IValue + DValue) / 4; - MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue; + MotorSpeed = pOne->MotorTargetSpeed - CorrectionValue; MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW); if (pOne->TurnParameter != 0) @@ -930,7 +923,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo) } pOne->MotorActualSpeed = (SBYTE)MotorSpeed; - MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue; + MotorSpeed = pTwo->MotorTargetSpeed + CorrectionValue; MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW); if (pOne->TurnParameter != 0) -- cgit v1.2.3