aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-01-21 23:40:15 +0100
committerNicolas Schodet2011-01-21 23:40:15 +0100
commit1b20bc3d4974a6f03031056910a95ca615a7a5ef (patch)
treef30fca89221147e7b1ec6a7c093723059c9f79ca
parent3b2647cd33fe16f372f1c3c99d16282ea0b74bbd (diff)
parentea4e008921f457d8111ed237f25b710817c6b34c (diff)
Merge branch 'jch'
-rw-r--r--AT91SAM7S256/Source/Functions.inl30
-rw-r--r--AT91SAM7S256/Source/c_cmd.c245
-rw-r--r--AT91SAM7S256/Source/c_input.c253
-rw-r--r--AT91SAM7S256/Source/c_output.c141
-rw-r--r--AT91SAM7S256/Source/c_ui.h2
-rw-r--r--AT91SAM7S256/Source/d_lowspeed.r444
-rw-r--r--AT91SAM7S256/Source/d_output.c1106
-rw-r--r--AT91SAM7S256/Source/d_output.h14
8 files changed, 752 insertions, 1483 deletions
diff --git a/AT91SAM7S256/Source/Functions.inl b/AT91SAM7S256/Source/Functions.inl
index 1ef019e..1424721 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 4ce9829..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)
@@ -1425,6 +1426,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);
@@ -1952,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 !!!
@@ -3233,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; i<NO_OF_COLORS; i++){
- RawArray[i] = pMapInput->Colors[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;
@@ -3426,11 +3441,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;
}
@@ -4603,7 +4618,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{
@@ -4941,20 +4956,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
{
@@ -6316,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);
}
@@ -6336,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);
}
@@ -6361,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);
@@ -6412,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..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;
@@ -226,8 +215,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 +231,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 +243,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);
}
@@ -287,158 +277,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) - 290];
+ 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),
@@ -449,7 +339,6 @@ void cInputCalcSensorValues(UBYTE No)
&(VarsInput.EdgeCnt[No]),
((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
-
}
break;
@@ -1135,13 +1024,6 @@ void cInputSetupType(UBYTE Port, UBYTE *pType, UBYTE OldType)
break;
case REFLECTION:
- {
- dInputSetActive(Port);
- dInputClearDigi0(Port);
- dInputClearDigi1(Port);
- }
- break;
-
case ANGLE:
{
dInputSetActive(Port);
@@ -1215,50 +1097,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;
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];
}
}
}
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]
diff --git a/AT91SAM7S256/Source/d_lowspeed.r b/AT91SAM7S256/Source/d_lowspeed.r
index 03826ea..279c10e 100644
--- a/AT91SAM7S256/Source/d_lowspeed.r
+++ b/AT91SAM7S256/Source/d_lowspeed.r
@@ -143,282 +143,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 */\
@@ -745,132 +498,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;\
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);