aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--AT91SAM7S256/Source/Functions.inl30
-rw-r--r--AT91SAM7S256/Source/c_cmd.c240
-rw-r--r--AT91SAM7S256/Source/c_cmd.h5
-rw-r--r--AT91SAM7S256/Source/c_input.c312
-rw-r--r--AT91SAM7S256/Source/c_output.c142
-rw-r--r--AT91SAM7S256/Source/c_output.iom2
-rw-r--r--AT91SAM7S256/Source/c_ui.h2
-rw-r--r--AT91SAM7S256/Source/d_lowspeed.r444
-rw-r--r--AT91SAM7S256/Source/d_output.c1249
-rw-r--r--AT91SAM7S256/Source/d_output.h14
10 files changed, 779 insertions, 1661 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 7483846..5370652 100644
--- a/AT91SAM7S256/Source/c_cmd.c
+++ b/AT91SAM7S256/Source/c_cmd.c
@@ -266,6 +266,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
TC_UBYTE, //IO_OUT_REG_D_VAL
TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
TC_SLONG, //IO_OUT_ROTATION_COUNT
+ TC_UBYTE, //IO_OUT_OPTIONS
//IO_OUT1
TC_UBYTE, //IO_OUT_FLAGS
@@ -283,6 +284,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
TC_UBYTE, //IO_OUT_REG_D_VAL
TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
TC_SLONG, //IO_OUT_ROTATION_COUNT
+ TC_UBYTE, //IO_OUT_OPTIONS
//IO_OUT2
TC_UBYTE, //IO_OUT_FLAGS
@@ -300,6 +302,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
TC_UBYTE, //IO_OUT_REG_D_VAL
TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
TC_SLONG, //IO_OUT_ROTATION_COUNT
+ TC_UBYTE, //IO_OUT_OPTIONS
};
@@ -461,32 +464,35 @@ 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];
+ pOut->Options = pOut->Mode & REG_METHOD;
+ memcpy((PSZ)(&(pOut->TachoLimit)), (PSZ)(&pInBuf[7]), 4);
+
+ pOut->Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
}
}
break;
@@ -502,12 +508,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 +532,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 +598,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 +613,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 +693,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 +1095,35 @@ 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);
+ IO_PTRS_OUT[IO_OUT_OPTIONS + i * IO_OUT_FPP] = (void*)&(pOut->Options);
}
//Initialize IO_PTRS_IN
for (i = 0; i < NO_OF_INPUTS; i++)
{
- IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorType);
- IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorMode);
- IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].ADRaw);
- IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorRaw);
- IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorValue);
- IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].InvalidData);
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
+ IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pIn->SensorType);
+ IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pIn->SensorMode);
+ IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pIn->ADRaw);
+ IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pIn->SensorRaw);
+ IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pIn->SensorValue);
+ IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pIn->InvalidData);
}
//Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd)
@@ -1962,29 +1968,31 @@ void cCmdResetDevices(void)
for (i = 0; i < NO_OF_INPUTS; i++)
{
+ INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
//Clear type and mode to defaults
- pMapInput->Inputs[i].SensorType = NO_SENSOR;
- pMapInput->Inputs[i].SensorMode = RAWMODE;
+ pIn->SensorType = NO_SENSOR;
+ pIn->SensorMode = RAWMODE;
//Reset input values to 0 prior to running (clear things like stale rotation counts)
- pMapInput->Inputs[i].ADRaw = 0;
- pMapInput->Inputs[i].SensorRaw = 0;
- pMapInput->Inputs[i].SensorValue = 0;
+ pIn->ADRaw = 0;
+ pIn->SensorRaw = 0;
+ pIn->SensorValue = 0;
//Assert invalid data flag so future code is aware of these changes
- pMapInput->Inputs[i].InvalidData = TRUE;
+ pIn->InvalidData = TRUE;
}
for (i = 0; i < NO_OF_OUTPUTS; i++)
{
//Coast and reset all motor parameters
- pMapOutPut->Outputs[i].Mode = 0;
- pMapOutPut->Outputs[i].RegMode = REGULATION_MODE_IDLE;
- pMapOutPut->Outputs[i].RunState = MOTOR_RUN_STATE_IDLE;
- pMapOutPut->Outputs[i].Speed = 0;
- pMapOutPut->Outputs[i].TachoLimit = 0;
- pMapOutPut->Outputs[i].SyncTurnParameter = 0;
- pMapOutPut->Outputs[i].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT;
+ OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
+ pOut->Mode = 0;
+ pOut->RegMode = REGULATION_MODE_IDLE;
+ pOut->RunState = MOTOR_RUN_STATE_IDLE;
+ pOut->Speed = 0;
+ pOut->TachoLimit = 0;
+ pOut->SyncTurnParameter = 0;
+ pOut->Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT;
}
//Lowspeed init, INSERT CODE !!!
@@ -3243,25 +3251,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;
@@ -3436,11 +3446,11 @@ ULONG cCmdGetSByte(void * pVal) {
ULONG cCmdGetFloat(void * pVal) {
float tempVal = *(float*)pVal;
- if (tempVal >= 0) {
- tempVal += 0.5;
+ if (tempVal >= 0.0f) {
+ tempVal += 0.5f;
}
else {
- tempVal -= 0.5;
+ tempVal -= 0.5f;
}
return (ULONG)tempVal;
}
@@ -4613,7 +4623,7 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode)
pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
// is number too big for display? then format differently and don't bother with trailing zeros
- if ((FltArgVal2 > 9999999999999.99)||(FltArgVal2 < -999999999999.99)){ // these are the widest %.2f numbers that will fit on display
+ if ((FltArgVal2 > 9999999999999.99f)||(FltArgVal2 < -999999999999.99f)){ // these are the widest %.2f numbers that will fit on display
strcpy (FormatString, "%.6g");
}
else{
@@ -4951,20 +4961,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
{
@@ -6326,10 +6336,12 @@ NXT_STATUS cCmdLSCheckStatus(UBYTE Port)
return (ERR_COMM_CHAN_INVALID);
}
+ INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
+
//If port is not configured properly ahead of time, report that error
//!!! This seems like the right policy, but may restrict otherwise valid read operations...
- if (!(pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED)
- || !(pMapInput->Inputs[Port].InvalidData == FALSE))
+ if (!(pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
+ || !(pInput->InvalidData == FALSE))
{
return (ERR_COMM_CHAN_NOT_READY);
}
@@ -6346,13 +6358,15 @@ UBYTE cCmdLSCalcBytesReady(UBYTE Port)
//Expect callers to validate Port, but short circuit here to be safe.
if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
return 0;
+
+ LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);
//Normally, bytes available is a simple difference.
- Tmp = pMapLowSpeed->InBuf[Port].InPtr - pMapLowSpeed->InBuf[Port].OutPtr;
+ Tmp = pInBuf->InPtr - pInBuf->OutPtr;
//If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage...
if (Tmp < 0)
- Tmp = (pMapLowSpeed->InBuf[Port].InPtr + (SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr));
+ Tmp = (pInBuf->InPtr + (SIZE_OF_LSBUF - pInBuf->OutPtr));
return (UBYTE)(Tmp);
}
@@ -6371,20 +6385,24 @@ NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseL
return (ERR_INVALID_SIZE);
}
+ INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
+ UBYTE * pChState = &(pMapLowSpeed->ChannelState[Port]);
+ LSBUF * pOutBuf = &(pMapLowSpeed->OutBuf[Port]);
+
//Only start writing process if port is properly configured and c_lowspeed module is ready
- if ((pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED)
- && (pMapInput->Inputs[Port].InvalidData == FALSE)
- && (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_ERROR))
+ if ((pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
+ && (pInput->InvalidData == FALSE)
+ && (*pChState == LOWSPEED_IDLE) || (*pChState == LOWSPEED_ERROR))
{
- pMapLowSpeed->OutBuf[Port].InPtr = 0;
- pMapLowSpeed->OutBuf[Port].OutPtr = 0;
+ pOutBuf->InPtr = 0;
+ pOutBuf->OutPtr = 0;
- memcpy(pMapLowSpeed->OutBuf[Port].Buf, pBuf, BufLength);
- pMapLowSpeed->OutBuf[Port].InPtr = (UBYTE)BufLength;
+ memcpy(pOutBuf->Buf, pBuf, BufLength);
+ pOutBuf->InPtr = (UBYTE)BufLength;
pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength;
- pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT;
+ *pChState = LOWSPEED_INIT;
pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port);
return (NO_ERR);
@@ -6422,18 +6440,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_cmd.h b/AT91SAM7S256/Source/c_cmd.h
index 86949cb..0b65764 100644
--- a/AT91SAM7S256/Source/c_cmd.h
+++ b/AT91SAM7S256/Source/c_cmd.h
@@ -194,10 +194,11 @@ enum
IO_OUT_REG_I_VAL,
IO_OUT_REG_D_VAL,
IO_OUT_BLOCK_TACH_COUNT,
- IO_OUT_ROTATION_COUNT
+ IO_OUT_ROTATION_COUNT,
+ IO_OUT_OPTIONS,
};
-#define IO_OUT_FPP 15
+#define IO_OUT_FPP 16
#define IO_OUT_FIELD_COUNT (IO_OUT_FPP * NO_OF_OUTPUTS)
//
diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c
index 47ca7c0..2cb9be1 100644
--- a/AT91SAM7S256/Source/c_input.c
+++ b/AT91SAM7S256/Source/c_input.c
@@ -66,63 +66,6 @@ enum
CUSTOM_SETUP = 0x80
};
-
-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,
- 690, 688, 684, 682, 678, 674, 672, 668, 666, 662, 660, 656, 654, 650, 648, 644,
- 642, 640, 638, 634, 632, 630, 628, 624, 622, 620, 616, 614, 612, 610, 608, 604,
- 602, 600, 598, 596, 592, 590, 588, 586, 584, 582, 580, 578, 576, 574, 572, 570,
- 568, 564, 562, 560, 558, 556, 554, 552, 550, 548, 546, 544, 542, 540, 538, 536,
- 534, 532, 530, 528, 526, 524, 522, 520, 518, 516, 514, 512, 510, 508, 508, 506,
- 504, 502, 500, 498, 496, 494, 494, 492, 490, 488, 486, 486, 484, 482, 480, 478,
- 476, 476, 474, 472, 470, 468, 468, 466, 464, 462, 460, 458, 458, 456, 454, 452,
- 450, 448, 448, 446, 444, 442, 442, 440, 438, 436, 436, 434, 432, 432, 430, 428,
- 426, 426, 424, 422, 420, 420, 418, 416, 416, 414, 412, 410, 408, 408, 406, 404,
- 404, 402, 400, 398, 398, 396, 394, 394, 392, 390, 390, 388, 386, 386, 384, 382,
- 382, 380, 378, 378, 376, 374, 374, 372, 370, 370, 368, 366, 366, 364, 362, 362,
- 360, 358, 358, 356, 354, 354, 352, 350, 350, 348, 348, 346, 344, 344, 342, 340,
- 340, 338, 338, 336, 334, 334, 332, 332, 330, 328, 328, 326, 326, 324, 322, 322,
- 320, 320, 318, 316, 316, 314, 314, 312, 310, 310, 308, 308, 306, 304, 304, 302,
- 300, 300, 298, 298, 296, 296, 294, 292, 292, 290, 290, 288, 286, 286, 284, 284,
- 282, 282, 280, 280, 278, 278, 276, 274, 274, 272, 272, 270, 270, 268, 268, 266,
- 264, 264, 262, 262, 260, 260, 258, 258, 256, 254, 254, 252, 252, 250, 250, 248,
- 248, 246, 244, 244, 242, 240, 240, 240, 238, 238, 236, 236, 234, 234, 232, 230,
- 230, 228, 228, 226, 226, 224, 224, 222, 220, 220, 218, 218, 216, 216, 214, 214,
- 212, 212, 210, 210, 208, 208, 206, 204, 204, 202, 202, 200, 200, 198, 198, 196,
- 196, 194, 194, 192, 190, 190, 188, 188, 186, 186, 184, 184, 182, 182, 180, 180,
- 178, 178, 176, 176, 174, 174, 172, 172, 170, 170, 168, 168, 166, 166, 164, 164,
- 162, 162, 160, 160, 158, 156, 156, 154, 154, 152, 152, 150, 150, 148, 148, 146,
- 146, 144, 144, 142, 142, 140, 140, 138, 136, 136, 136, 134, 134, 132, 130, 130,
- 128, 128, 126, 126, 124, 124, 122, 122, 120, 120, 118, 118, 116, 116, 114, 114,
- 112, 110, 110, 108, 108, 106, 106, 104, 104, 102, 102, 100, 100, 98, 98, 96,
- 94, 94, 92, 92, 90, 90, 88, 88, 86, 86, 84, 82, 82, 80, 80, 78,
- 78, 76, 76, 74, 74, 72, 72, 70, 70, 68, 68, 66, 66, 64, 62, 62,
- 60, 60, 58, 56, 56, 54, 54, 52, 52, 50, 50, 48, 48, 46, 46, 44,
- 44, 42, 40, 40, 38, 38, 36, 34, 34, 32, 32, 30, 30, 28, 28, 26,
- 24, 24, 22, 22, 20, 20, 18, 16, 16, 14, 14, 12, 12, 10, 10, 8,
- 6, 6, 4, 2, 2, 0, 0, -2, -4, -4, -6, -6, -8, -10, -10, -12,
- -12, -14, -16, - 16, -18, -20, -20, -22, -22, -24, -26, -26, -28, -30, -30, -32,
- -34, -34, -36, -36, -38, -40, -40, -42, -42, -44, -46, -46, -48, -50, -50, -52,
- -54, -54, -56, -58, -58, -60, -60, -62, -64, -66, -66, -68, -70, -70, -72, -74,
- -76, -76, -78, -80, -80, -82, -84, -86, -86, -88, -90, -90, -92, -94, -94, -96,
- -98, -98, -100, -102, -104, -106, -106, -108, -110, -112, -114, -114, -116, -118, -120, -120,
- -122, -124, -126, -128, -130, -130, -132, -134, -136, -138, -140, -142, -144, -146, -146, -148,
- -150, -152, -154, -156, -158, -160, -162, -164, -166, -166, -168, -170, -172, -174, -176, -178,
- -180, -182, -184, -186, -188, -190, -192, -194, -196, -196, -198, -200, -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
-};
-
static IOMAPINPUT IOMapInput;
static VARSINPUT VarsInput;
@@ -150,6 +93,7 @@ void cInputSetupCustomSensor(UBYTE Port);
void cInputCalcSensorValues(UBYTE No);
UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus);
void cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals);
+SWORD cInputTempConv(UWORD InputVal);
void cInputInit(void* pHeader)
{
@@ -226,8 +170,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 +186,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 +198,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 +232,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 = cInputTempConv(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 +294,6 @@ void cInputCalcSensorValues(UBYTE No)
&(VarsInput.EdgeCnt[No]),
((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK),
((IOMapInput.Inputs[No].SensorMode) & MODEMASK));
-
}
break;
@@ -1135,13 +979,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 +1052,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;
@@ -1324,6 +1120,22 @@ void cInputSetupCustomSensor(UBYTE Port)
}
+SWORD cInputTempConv(UWORD InputVal)
+{
+ static const long long TempCoeff[] = { -5425ll, 9261399ll, -6686663252ll,
+ 2573629857807ll, -822478326197838ll, 195856762719738784ll };
+ const unsigned int TempCoeffShift = 48;
+ /* Replace the original table with polynomial. */
+ int i;
+ long long Input = InputVal;
+ long long Output = TempCoeff[0];
+ for (i = 1; i < sizeof TempCoeff / sizeof TempCoeff[0]; i++)
+ Output = Output * Input + TempCoeff[i];
+ /* Round. */
+ return Output + (1ll << TempCoeffShift - 1) >> TempCoeffShift;
+}
+
+
UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus)
{
diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c
index 9566938..4601ff3 100644
--- a/AT91SAM7S256/Source/c_output.c
+++ b/AT91SAM7S256/Source/c_output.c
@@ -45,15 +45,17 @@ 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;
+ pOut->Options = 0x00;
}
VarsOutput.TimeCnt = 0;
dOutputInit();
@@ -65,72 +67,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 +154,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_output.iom b/AT91SAM7S256/Source/c_output.iom
index 80e35de..fc26efd 100644
--- a/AT91SAM7S256/Source/c_output.iom
+++ b/AT91SAM7S256/Source/c_output.iom
@@ -72,7 +72,7 @@ typedef struct
UBYTE RegMode; /* RW - Tells which regulation mode should be used */
UBYTE Overloaded; /* R - True if the motor has been overloaded within speed control regulation */
SBYTE SyncTurnParameter; /* RW - Holds the turning parameter need within MoveBlock */
- UBYTE SpareOne;
+ UBYTE Options;
UBYTE SpareTwo;
UBYTE SpareThree;
}OUTPUT;
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..3608cb7 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -25,7 +25,6 @@
#define MAX_COUNT_TO_RUN 10000000
#define REG_MAX_VALUE 100
-#define REG_MIN_VALUE -100
#define RAMP_TIME_INTERVAL 25 // Measured in 1 mS => 25 mS interval
#define REGULATION_TIME 100 // Measured in 1 mS => 100 mS regulation interval
@@ -36,6 +35,7 @@
#define COAST_MOTOR_MODE 0
void dOutputRampDownSynch(UBYTE MotorNr);
+SLONG dOutputBound(SLONG In, SLONG Limit);
typedef struct
{
@@ -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,56 +715,70 @@ 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++;
}
}
}
+/* Check if Value is between [-Limit:Limit], and change it if it is not the case. */
+SLONG dOutputBound(SLONG Value, SLONG Limit)
+{
+ if (Value > Limit)
+ return Limit;
+ else if (Value < -Limit)
+ return -Limit;
+ else
+ return Value;
+}
+
/* Function called to evaluate which regulation princip that need to run and which MotorNr to use (I.E.: Which motors are synched together)*/
void dOutputRegulateMotor(UBYTE MotorNr)
{
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);
@@ -767,88 +794,47 @@ void dOutputRegulateMotor(UBYTE MotorNr)
/* The regulation form only control one motor at a time */
void dOutputCalculateMotorPosition(UBYTE MotorNr)
{
- SWORD PositionError;
- SWORD PValue;
- SWORD IValue;
- SWORD DValue;
- SWORD TotalRegValue;
- SWORD NewSpeedCount = 0;
+ SLONG PositionError;
+ SLONG PValue;
+ SLONG IValue;
+ SLONG DValue;
+ SLONG TotalRegValue;
+ SLONG NewSpeedCount = 0;
- 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 = (pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;
//Overflow control on PositionError
- if (MotorData[MotorNr].RegPParameter != 0)
- {
- if (PositionError > (SWORD)(32000 / MotorData[MotorNr].RegPParameter))
- {
- PositionError = (SWORD)(32000 / MotorData[MotorNr].RegPParameter);
- }
- if (PositionError < (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter)))
- {
- PositionError = (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter));
- }
- }
- else
- {
- if (PositionError > (SWORD)32000)
- {
- PositionError = (SWORD)32000;
- }
- if (PositionError < (SWORD)-32000)
- {
- PositionError = (SWORD)-32000;
- }
- }
+ PositionError = dOutputBound (PositionError, 32000);
- PValue = PositionError * (SWORD)(MotorData[MotorNr].RegPParameter/REG_CONST_DIV);
- if (PValue > (SWORD)REG_MAX_VALUE)
- {
- PValue = REG_MAX_VALUE;
- }
- if (PValue <= (SWORD)REG_MIN_VALUE)
- {
- PValue = REG_MIN_VALUE;
- }
+ PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV);
+ PValue = dOutputBound (PValue, REG_MAX_VALUE);
- DValue = (PositionError - MotorData[MotorNr].OldPositionError) * (SWORD)(MotorData[MotorNr].RegDParameter/REG_CONST_DIV);
- MotorData[MotorNr].OldPositionError = PositionError;
+ DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV);
+ pMD->OldPositionError = (SWORD)PositionError;
- MotorData[MotorNr].AccError = (MotorData[MotorNr].AccError * 3) + PositionError;
- MotorData[MotorNr].AccError = MotorData[MotorNr].AccError / 4;
+ pMD->AccError = (pMD->AccError * 3) + (SWORD)PositionError;
+ pMD->AccError = pMD->AccError / 4;
+ pMD->AccError = dOutputBound (pMD->AccError, 800);
- if (MotorData[MotorNr].AccError > (SWORD)800)
- {
- MotorData[MotorNr].AccError = 800;
- }
- if (MotorData[MotorNr].AccError <= (SWORD)-800)
- {
- MotorData[MotorNr].AccError = -800;
- }
- IValue = MotorData[MotorNr].AccError * (SWORD)(MotorData[MotorNr].RegIParameter/REG_CONST_DIV);
+ IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV);
+ IValue = dOutputBound (IValue, REG_MAX_VALUE);
- if (IValue > (SWORD)REG_MAX_VALUE)
- {
- IValue = REG_MAX_VALUE;
- }
- if (IValue <= (SWORD)REG_MIN_VALUE)
- {
- IValue = REG_MIN_VALUE;
- }
- TotalRegValue = (SWORD)((PValue + IValue + DValue)/2);
+ TotalRegValue = (PValue + IValue + DValue) / 2;
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 */
@@ -856,44 +842,46 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
{
SLONG TempTurnParameter;
- SWORD PValue;
- SWORD IValue;
- SWORD DValue;
- SWORD CorrectionValue;
- SWORD MotorSpeed;
+ SLONG PValue;
+ SLONG IValue;
+ SLONG DValue;
+ SLONG CorrectionValue;
+ SLONG MotorSpeed;
- 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
@@ -905,291 +893,94 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
//SyncTurnParameter should ophold difference between the two motors.
SyncData.SyncTachoDif += SyncData.SyncTurnParameter;
+ SyncData.SyncTachoDif = dOutputBound (SyncData.SyncTachoDif, 500);
- if (SyncData.SyncTachoDif > 500)
- {
- SyncData.SyncTachoDif = 500;
- }
- if (SyncData.SyncTachoDif < -500)
- {
- SyncData.SyncTachoDif = -500;
- }
+ PValue = SyncData.SyncTachoDif * (pOne->RegPParameter/REG_CONST_DIV);
- /*
- if ((SWORD)SyncData.SyncTachoDif > 500)
- {
- SyncData.SyncTachoDif = 500;
- }
- if ((SWORD)SyncData.SyncTachoDif < -500)
- {
- SyncData.SyncTachoDif = -500;
- }
- */
-
- PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(MotorData[MotorOne].RegPParameter/REG_CONST_DIV);
-
- DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(MotorData[MotorOne].RegDParameter/REG_CONST_DIV);
+ DValue = (SyncData.SyncTachoDif - SyncData.SyncOldError) * (pOne->RegDParameter/REG_CONST_DIV);
SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif;
SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif;
+ SyncData.SyncAccError = dOutputBound (SyncData.SyncAccError, 900);
- if (SyncData.SyncAccError > (SWORD)900)
- {
- SyncData.SyncAccError = 900;
- }
- if (SyncData.SyncAccError < (SWORD)-900)
- {
- SyncData.SyncAccError = -900;
- }
- IValue = SyncData.SyncAccError * (SWORD)(MotorData[MotorOne].RegIParameter/REG_CONST_DIV);
-
- CorrectionValue = (SWORD)((PValue + IValue + DValue)/4);
+ IValue = SyncData.SyncAccError * (pOne->RegIParameter/REG_CONST_DIV);
- MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed - CorrectionValue;
+ CorrectionValue = (PValue + IValue + DValue) / 4;
- if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
- {
- MotorSpeed = MAXIMUM_SPEED_FW;
- }
- else
- {
- if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
- {
- MotorSpeed = MAXIMUM_SPEED_RW;
- }
- }
+ MotorSpeed = pOne->MotorTargetSpeed - CorrectionValue;
+ MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
- if (MotorData[MotorOne].TurnParameter != 0)
+ if (pOne->TurnParameter != 0)
{
- if (MotorData[MotorOne].MotorTargetSpeed > 0)
+ if (pOne->MotorTargetSpeed > 0)
{
- if (MotorSpeed > (SWORD)MotorData[MotorOne].MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed < (SWORD)-MotorData[MotorOne].MotorTargetSpeed)
- {
- MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, pOne->MotorTargetSpeed);
}
else
{
- if (MotorSpeed < (SWORD)MotorData[MotorOne].MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed > (SWORD)-MotorData[MotorOne].MotorTargetSpeed)
- {
- MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, -pOne->MotorTargetSpeed);
}
}
- MotorData[MotorOne].MotorActualSpeed = (SBYTE)MotorSpeed;
+ pOne->MotorActualSpeed = (SBYTE)MotorSpeed;
- MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed + CorrectionValue;
+ MotorSpeed = pTwo->MotorTargetSpeed + CorrectionValue;
+ MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
- if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
+ if (pOne->TurnParameter != 0)
{
- MotorSpeed = MAXIMUM_SPEED_FW;
- }
- else
- {
- if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
+ if (pTwo->MotorTargetSpeed > 0)
{
- MotorSpeed = MAXIMUM_SPEED_RW;
- }
- }
-
- if (MotorData[MotorOne].TurnParameter != 0)
- {
- if (MotorData[MotorTwo].MotorTargetSpeed > 0)
- {
- if (MotorSpeed > (SWORD)MotorData[MotorTwo].MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed < (SWORD)-MotorData[MotorTwo].MotorTargetSpeed)
- {
- MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, pTwo->MotorTargetSpeed);
}
else
{
- if (MotorSpeed < (SWORD)MotorData[MotorTwo].MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed > (SWORD)-MotorData[MotorTwo].MotorTargetSpeed)
- {
- MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (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 +990,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 +1144,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 +1179,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);