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