aboutsummaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorJohn Hansen2011-03-13 23:36:50 +0000
committerNicolas Schodet2011-07-04 00:37:04 +0200
commitfc1e658f15bbb8d4497d189e79b43fab4b964839 (patch)
tree829ae4b3ece944522a68919cd2685d49fc06c341
parentd8a6b2de30e139a676593aac5feb9eb47479dce2 (diff)
Added position regulation mode (Nicolas Schodet)
git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc/branches/version_131@37 c9361245-7fe8-9947-84e8-057757c4e366
-rw-r--r--AT91SAM7S256/Source/c_cmd.c8
-rw-r--r--AT91SAM7S256/Source/c_cmd.h4
-rw-r--r--AT91SAM7S256/Source/c_output.c19
-rw-r--r--AT91SAM7S256/Source/c_output.iom14
-rw-r--r--AT91SAM7S256/Source/d_output.c419
-rw-r--r--AT91SAM7S256/Source/d_output.h16
6 files changed, 284 insertions, 196 deletions
diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c
index 231ebec..7a81c71 100644
--- a/AT91SAM7S256/Source/c_cmd.c
+++ b/AT91SAM7S256/Source/c_cmd.c
@@ -320,6 +320,8 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
TC_SLONG, //IO_OUT_ROTATION_COUNT
TC_UBYTE, //IO_OUT_OPTIONS
+ TC_SBYTE, //IO_OUT_MAX_SPEED
+ TC_SBYTE, //IO_OUT_MAX_ACCELERATION
//IO_OUT1
TC_UBYTE, //IO_OUT_FLAGS
@@ -338,6 +340,8 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
TC_SLONG, //IO_OUT_ROTATION_COUNT
TC_UBYTE, //IO_OUT_OPTIONS
+ TC_SBYTE, //IO_OUT_MAX_SPEED
+ TC_SBYTE, //IO_OUT_MAX_ACCELERATION
//IO_OUT2
TC_UBYTE, //IO_OUT_FLAGS
@@ -356,6 +360,8 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
TC_SLONG, //IO_OUT_ROTATION_COUNT
TC_UBYTE, //IO_OUT_OPTIONS
+ TC_SBYTE, //IO_OUT_MAX_SPEED
+ TC_SBYTE, //IO_OUT_MAX_ACCELERATION
};
@@ -1402,6 +1408,8 @@ void cCmdInit(void* pHeader)
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);
+ IO_PTRS_OUT[IO_OUT_MAX_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->MaxSpeed);
+ IO_PTRS_OUT[IO_OUT_MAX_ACCELERATION + i * IO_OUT_FPP] = (void*)&(pOut->MaxAcceleration);
}
//Initialize IO_PTRS_IN
diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h
index a555e8b..bb4891f 100644
--- a/AT91SAM7S256/Source/c_cmd.h
+++ b/AT91SAM7S256/Source/c_cmd.h
@@ -207,9 +207,11 @@ enum
IO_OUT_BLOCK_TACH_COUNT,
IO_OUT_ROTATION_COUNT,
IO_OUT_OPTIONS,
+ IO_OUT_MAX_SPEED,
+ IO_OUT_MAX_ACCELERATION,
};
-#define IO_OUT_FPP 16
+#define IO_OUT_FPP 18
#define IO_OUT_FIELD_COUNT (IO_OUT_FPP * NO_OF_OUTPUTS)
//
diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c
index 212cc89..9e7c2e0 100644
--- a/AT91SAM7S256/Source/c_output.c
+++ b/AT91SAM7S256/Source/c_output.c
@@ -43,7 +43,6 @@ void cOutputInit(void* pHeader)
{
UBYTE Tmp;
- IOMapOutput.PwnFreq = REGULATION_TIME;
for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++)
{
OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]);
@@ -57,7 +56,11 @@ void cOutputInit(void* pHeader)
pOut->RegIParameter = DEFAULT_I_GAIN_FACTOR;
pOut->RegDParameter = DEFAULT_D_GAIN_FACTOR;
pOut->Options = 0x00;
+ pOut->MaxSpeed = DEFAULT_MAX_SPEED;
+ pOut->MaxAcceleration = DEFAULT_MAX_ACCELERATION;
}
+ IOMapOutput.RegulationTime = REGULATION_TIME;
+ IOMapOutput.RegulationOptions = 0;
VarsOutput.TimeCnt = 0;
dOutputInit();
}
@@ -66,7 +69,6 @@ void cOutputCtrl(void)
{
UBYTE Tmp;
- dOutputUpdateRegulationTime(IOMapOutput.PwnFreq);
for(Tmp = 0; Tmp < NO_OF_OUTPUTS; Tmp++)
{
OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]);
@@ -95,11 +97,6 @@ void cOutputCtrl(void)
dOutputSetSpeed (Tmp, pOut->RunState, pOut->Speed, pOut->SyncTurnParameter);
}
}
- if (pOut->Flags & UPDATE_TACHO_LIMIT)
- {
- pOut->Flags &= ~UPDATE_TACHO_LIMIT;
- dOutputSetTachoLimit(Tmp, pOut->TachoLimit, pOut->Options);
- }
if (pOut->Flags & UPDATE_MODE)
{
pOut->Flags &= ~UPDATE_MODE;
@@ -130,13 +127,21 @@ void cOutputCtrl(void)
dOutputDisableRegulation(Tmp);
}
}
+ if (pOut->Flags & UPDATE_TACHO_LIMIT)
+ {
+ pOut->Flags &= ~UPDATE_TACHO_LIMIT;
+ dOutputSetTachoLimit(Tmp, pOut->TachoLimit, pOut->Options);
+ }
if (pOut->Flags & UPDATE_PID_VALUES)
{
pOut->Flags &= ~UPDATE_PID_VALUES;
dOutputSetPIDParameters(Tmp, pOut->RegPParameter, pOut->RegIParameter, pOut->RegDParameter);
+ dOutputSetMax(Tmp, pOut->MaxSpeed, pOut->MaxAcceleration);
}
}
}
+ dOutputSetRegulationTime(IOMapOutput.RegulationTime);
+ dOutputSetRegulationOptions(IOMapOutput.RegulationOptions);
dOutputCtrl();
cOutputUpdateIomap();
}
diff --git a/AT91SAM7S256/Source/c_output.iom b/AT91SAM7S256/Source/c_output.iom
index f6191bf..989a0c4 100644
--- a/AT91SAM7S256/Source/c_output.iom
+++ b/AT91SAM7S256/Source/c_output.iom
@@ -52,9 +52,10 @@ enum
// Constant related to RegMode
enum
{
- REGULATION_MODE_IDLE,
- REGULATION_MODE_MOTOR_SPEED,
- REGULATION_MODE_MOTOR_SYNC
+ REGULATION_MODE_IDLE = 0,
+ REGULATION_MODE_MOTOR_SPEED = 1,
+ REGULATION_MODE_MOTOR_SYNC = 2,
+ REGULATION_MODE_MOTOR_POS = 4,
};
typedef struct
@@ -76,15 +77,16 @@ typedef struct
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 Options;
- UBYTE SpareTwo;
- UBYTE SpareThree;
+ SBYTE MaxSpeed; /* RW - Maximum speed for absolute regulation, or 0 for no limit */
+ SBYTE MaxAcceleration; /* RW - Maximum acceleration for absolute regulation, or 0 for no limit */
}OUTPUT;
typedef struct
{
OUTPUT Outputs[NO_OF_OUTPUTS];
- UBYTE PwnFreq; // use for frequency of checking regulation mode
+ UBYTE RegulationTime; /* RW - Interval between regulation computations */
+ UBYTE RegulationOptions; /* RW - Options for regulation, see REGOPTION_* */
}IOMAPOUTPUT;
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 73719cc..3e9e1cd 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -17,26 +17,38 @@
#include "d_output.h"
#include "d_output.r"
+#include <math.h>
+
#define MAXIMUM_SPEED_FW 100
#define MAXIMUM_SPEED_RW -100
#define INPUT_SCALE_FACTOR 100
+#define SPEED_TIME 100
#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 RAMPDOWN_STATE_RAMPDOWN 0
#define RAMPDOWN_STATE_CONTINIUE 1
#define COAST_MOTOR_MODE 0
+void dOutputRampDownSynch(UBYTE MotorNr);
+SLONG dOutputBound(SLONG In, SLONG Limit);
+SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError);
+SLONG dOutputFractionalChange(SLONG Value, SWORD *FracError);
+void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff);
+
+#define ABS(a) (((a) < 0) ? -(a) : (a))
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+
#define OPTION_HOLDATLIMIT 0x10
#define OPTION_RAMPDOWNTOLIMIT 0x20
-void dOutputRampDownSynch(UBYTE MotorNr);
-
typedef struct
{
SBYTE MotorSetSpeed; // Motor setpoint in speed
@@ -57,6 +69,7 @@ typedef struct
SWORD MotorRampUpIncrement; // Tell the number of count between each speed adjustment during Ramp-up
SWORD AccError; // Accumulated Error, used within the integrator of the PID regulation
SWORD OldPositionError; // Used within position regulation
+ SWORD PositionFracError; // Fractionnal position error of last position update
SLONG DeltaCaptureCount; // Counts within last regulation time-periode
SLONG CurrentCaptureCount; // Total counts since motor counts has been reset
SLONG MotorTachoCountToRun; // Holds number of counts to run. 0 = Run forever
@@ -64,6 +77,10 @@ typedef struct
SLONG MotorRampTachoCountOld; // Used to hold old position during Ramp-Up
SLONG MotorRampTachoCountStart; // Used to hold position when Ramp-up started
SLONG RotationCaptureCount; // Counter for additional rotation counter
+ SLONG MotorTachoCountTarget; // For absolute regulation, position on which regulation is done
+ SWORD SpeedFracError; // Fractionnal speed error of last speed update
+ SBYTE MotorMaxSpeed; // For absolute regulation, maximum motor speed
+ SBYTE MotorMaxAcceleration; // For absolute regulation, maximum motor acceleration
UBYTE RunStateAtLimit; // what run state to switch to when tacho limit is reached
UBYTE RampDownToLimit;
UBYTE Spare2;
@@ -80,8 +97,8 @@ typedef struct
static MOTORDATA MotorData[3];
static SYNCMOTORDATA SyncData;
-
-static UBYTE RegTime;
+static UBYTE RegulationTime;
+static UBYTE RegulationOptions;
UBYTE dOutputRunStateAtLimit(MOTORDATA * pMD)
{
@@ -111,7 +128,7 @@ void dOutputInit(void)
ENABLECaptureMotorB;
ENABLECaptureMotorC;
- RegTime = REGULATION_TIME;
+ RegulationTime = REGULATION_TIME;
for (Temp = 0; Temp < 3; Temp++)
{
@@ -125,10 +142,13 @@ void dOutputInit(void)
pMD->MotorTachoCountToRun = 0;
pMD->MotorRunForever = 1;
pMD->AccError = 0;
+ pMD->PositionFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->RegPParameter = DEFAULT_P_GAIN_FACTOR;
pMD->RegIParameter = DEFAULT_I_GAIN_FACTOR;
pMD->RegDParameter = DEFAULT_D_GAIN_FACTOR;
+ pMD->MotorMaxSpeed = DEFAULT_MAX_SPEED;
+ pMD->MotorMaxAcceleration = DEFAULT_MAX_ACCELERATION;
pMD->RegulationMode = 0;
pMD->MotorOverloaded = 0;
pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE;
@@ -181,11 +201,12 @@ void dOutputCtrl(void)
pMD->MotorSetSpeed = 0;
pMD->MotorActualSpeed = 0;
pMD->MotorTargetSpeed = 0;
+ pMD->PositionFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
pMD->MotorRunState = MOTOR_RUN_STATE_RUNNING;
}
- if (pMD->RegulationTimeCount > RegTime)
+ if (pMD->RegulationTimeCount > RegulationTime)
{
pMD->RegulationTimeCount = 0;
dOutputRegulateMotor(MotorNr);
@@ -237,6 +258,7 @@ void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode)
{
pMD->AccError = 0;
pMD->OldPositionError = 0;
+ pMD->PositionFracError = 0;
}
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
@@ -265,6 +287,7 @@ void dOutputResetTachoLimit(UBYTE MotorNr)
MOTORDATA * pMD = &(MotorData[MotorNr]);
pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
+ pMD->MotorTachoCountTarget = 0;
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
{
@@ -300,13 +323,38 @@ void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewReg
pMD->RegDParameter = NewRegDParameter;
}
+/* Set maximum speed and acceleration */
+void dOutputSetMax(UBYTE MotorNr, SBYTE NewMaxSpeed, SBYTE NewMaxAcceleration)
+{
+ MOTORDATA * pMD = &(MotorData[MotorNr]);
+ pMD->MotorMaxSpeed = NewMaxSpeed;
+ pMD->MotorMaxAcceleration = NewMaxAcceleration;
+}
+
+/* Set new regulation time */
+void dOutputSetRegulationTime(UBYTE NewRegulationTime)
+{
+ RegulationTime = NewRegulationTime;
+}
+
+/* Set new regulation options */
+void dOutputSetRegulationOptions(UBYTE NewRegulationOptions)
+{
+ RegulationOptions = NewRegulationOptions;
+}
+
/* Called to set TachoCountToRun which is used for position control for the model */
/* Must be called before motor start */
/* TachoCountToRun is calculated as a signed value */
void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel, UBYTE Options)
{
MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (BlockTachoCntToTravel == 0)
+ if (pMD->RegulationMode & REGSTATE_POSITION)
+ {
+ pMD->MotorRunForever = 0;
+ pMD->MotorTachoCountToRun = BlockTachoCntToTravel;
+ }
+ else if (BlockTachoCntToTravel == 0)
{
pMD->MotorRunForever = 1;
pMD->RunStateAtLimit = MOTOR_RUN_STATE_IDLE;
@@ -354,6 +402,7 @@ void dOutputSetSpeed (UBYTE MotorNr, UBYTE NewMotorRunState, SBYTE Speed, SBYTE
{
pMD->AccError = 0;
pMD->OldPositionError = 0;
+ pMD->PositionFracError = 0;
pMD->RegulationTimeCount = 0;
pMD->DeltaCaptureCount = 0;
TACHOCountReset(MotorNr);
@@ -696,6 +745,11 @@ void dOutputRampDownFunction(UBYTE MotorNr)
void dOutputTachoLimitControl(UBYTE MotorNr)
{
MOTORDATA * pMD = &(MotorData[MotorNr]);
+ if (pMD->RegulationMode & REGSTATE_POSITION)
+ {
+ /* No limit when doing absolute position regulation. */
+ return;
+ }
if (pMD->MotorRunForever == 0)
{
if (pMD->RegulationMode & REGSTATE_SYNCHRONE)
@@ -811,6 +865,17 @@ void dOutputMotorIdleControl(UBYTE MotorNr)
}
}
+/* Check if Value is between [-Limit:Limit], and change it if it is not the case. */
+SLONG dOutputBound(SLONG Value, SLONG Limit)
+{
+ if (Value > Limit)
+ return Limit;
+ else if (Value < -Limit)
+ return -Limit;
+ else
+ return Value;
+}
+
/* Function called to evaluate which regulation princip that need to run and which MotorNr to use (I.E.: Which motors are synched together)*/
void dOutputRegulateMotor(UBYTE MotorNr)
{
@@ -818,7 +883,11 @@ void dOutputRegulateMotor(UBYTE MotorNr)
UBYTE SyncMotorTwo;
MOTORDATA * pMD = &(MotorData[MotorNr]);
- if (pMD->RegulationMode & REGSTATE_REGULATED)
+ if (pMD->RegulationMode & REGSTATE_POSITION)
+ {
+ dOutputAbsolutePositionRegulation(MotorNr);
+ }
+ else if (pMD->RegulationMode & REGSTATE_REGULATED)
{
dOutputCalculateMotorPosition(MotorNr);
}
@@ -836,92 +905,167 @@ void dOutputRegulateMotor(UBYTE MotorNr)
}
}
-/* Regulation function used when Position regulation is enabled */
-/* The regulation form only control one motor at a time */
-void dOutputCalculateMotorPosition(UBYTE MotorNr)
+/* Compute PID regulation result for a given error. */
+SLONG dOutputPIDRegulation(UBYTE MotorNr, SLONG PositionError)
{
- SWORD PositionError;
- SWORD PValue;
- SWORD IValue;
- SWORD DValue;
- SWORD TotalRegValue;
- SWORD NewSpeedCount = 0;
+ SLONG PValue, DValue, IValue, TotalRegValue;
- MOTORDATA * pMD = &(MotorData[MotorNr]);
- NewSpeedCount = (SWORD)((pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);
+ MOTORDATA *pMD = &MotorData[MotorNr];
+
+ PositionError = dOutputBound (PositionError, 32000);
+
+ PValue = PositionError * (pMD->RegPParameter/REG_CONST_DIV);
+
+ DValue = (PositionError - pMD->OldPositionError) * (pMD->RegDParameter/REG_CONST_DIV);
+ pMD->OldPositionError = PositionError;
- PositionError = (SWORD)(pMD->OldPositionError - pMD->DeltaCaptureCount) + NewSpeedCount;
+ pMD->AccError = (pMD->AccError * 3 + PositionError) / 4;
+ pMD->AccError = dOutputBound (pMD->AccError, 800);
- //Overflow control on PositionError
- if (pMD->RegPParameter != 0)
+ IValue = pMD->AccError * (pMD->RegIParameter/REG_CONST_DIV);
+
+ if (!(RegulationOptions & REGOPTION_NO_SATURATION))
{
- if (PositionError > (SWORD)(32000 / pMD->RegPParameter))
- {
- PositionError = (SWORD)(32000 / pMD->RegPParameter);
- }
- if (PositionError < (SWORD)(-(32000 / pMD->RegPParameter)))
- {
- PositionError = (SWORD)(-(32000 / pMD->RegPParameter));
- }
+ PValue = dOutputBound (PValue, REG_MAX_VALUE);
+ IValue = dOutputBound (IValue, REG_MAX_VALUE);
}
- else
+
+ TotalRegValue = (PValue + IValue + DValue) / 2;
+
+ if (TotalRegValue > MAXIMUM_SPEED_FW)
{
- if (PositionError > (SWORD)32000)
- {
- PositionError = (SWORD)32000;
- }
- if (PositionError < (SWORD)-32000)
- {
- PositionError = (SWORD)-32000;
- }
+ TotalRegValue = MAXIMUM_SPEED_FW;
+ pMD->MotorOverloaded = 1;
+ }
+ else if (TotalRegValue < MAXIMUM_SPEED_RW)
+ {
+ TotalRegValue = MAXIMUM_SPEED_RW;
+ pMD->MotorOverloaded = 1;
}
- PValue = PositionError * (SWORD)(pMD->RegPParameter/REG_CONST_DIV);
- if (PValue > (SWORD)REG_MAX_VALUE)
+ return TotalRegValue;
+}
+
+/* Compute integer change for this regulation step, according to value and
+ * previous fractional error.
+ * Used for values which are expressed as "per SPEED_TIME" to translate them
+ * in "per RegulationTime".*/
+SLONG dOutputFractionalChange(SLONG Value, SWORD *FracError)
+{
+ SLONG IntegerChange;
+
+ /* Apply fractional change in case RegulationTime is different from
+ * SPEED_TIME. In this case, fractional part is accumulated until it reach
+ * one half (with "one" being SPEED_TIME). This is use the same principle
+ * as the Bresenham algorithm. */
+ IntegerChange = Value * RegulationTime / SPEED_TIME;
+ *FracError += Value * RegulationTime % SPEED_TIME;
+ if (*FracError > SPEED_TIME / 2)
{
- PValue = REG_MAX_VALUE;
+ *FracError -= SPEED_TIME;
+ IntegerChange++;
}
- if (PValue <= (SWORD)REG_MIN_VALUE)
+ else if (*FracError < -SPEED_TIME / 2)
{
- PValue = REG_MIN_VALUE;
+ *FracError += SPEED_TIME;
+ IntegerChange--;
}
- DValue = (PositionError - pMD->OldPositionError) * (SWORD)(pMD->RegDParameter/REG_CONST_DIV);
- pMD->OldPositionError = PositionError;
-
- pMD->AccError = (pMD->AccError * 3) + PositionError;
- pMD->AccError = pMD->AccError / 4;
+ return IntegerChange;
+}
- if (pMD->AccError > (SWORD)800)
+/* Filter speed according to motor maximum speed and acceleration. */
+void dOutputSpeedFilter(UBYTE MotorNr, SLONG PositionDiff)
+{
+ /* Inputs:
+ * - PositionDiff: difference between current position and position to reach.
+ * - MotorMaxAcceleration: maximum speed change per regulation period (or 0 for unlimited).
+ * - MotorMaxSpeed: maximum motor speed (can not be zero, or do not call this function).
+ * Output:
+ * - MotorTargetSpeed: speed to regulate on motor.
+ */
+ MOTORDATA *pMD = &MotorData[MotorNr];
+ SLONG IdealSpeed;
+ SLONG PositionDiffAbs = ABS (PositionDiff);
+ /* Should be able to brake on time. */
+ if (pMD->MotorMaxAcceleration
+ && PositionDiffAbs < MAXIMUM_SPEED_FW * MAXIMUM_SPEED_FW / 2)
{
- pMD->AccError = 800;
+ IdealSpeed = sqrtf (2 * PositionDiffAbs * pMD->MotorMaxAcceleration);
+ IdealSpeed = dOutputBound (IdealSpeed, pMD->MotorMaxSpeed);
}
- if (pMD->AccError <= (SWORD)-800)
+ else
{
- pMD->AccError = -800;
+ /* Do not go past consign. */
+ IdealSpeed = MIN (PositionDiffAbs, pMD->MotorMaxSpeed);
}
- IValue = pMD->AccError * (SWORD)(pMD->RegIParameter/REG_CONST_DIV);
-
- if (IValue > (SWORD)REG_MAX_VALUE)
+ /* Apply sign. */
+ if (PositionDiff < 0)
{
- IValue = REG_MAX_VALUE;
+ IdealSpeed = -IdealSpeed;
}
- if (IValue <= (SWORD)REG_MIN_VALUE)
+ /* Check max acceleration. */
+ SLONG SpeedDiff = IdealSpeed - pMD->MotorTargetSpeed;
+ if (pMD->MotorMaxAcceleration)
{
- IValue = REG_MIN_VALUE;
+ SLONG MaxSpeedChange = dOutputFractionalChange (pMD->MotorMaxAcceleration, &pMD->SpeedFracError);
+ SpeedDiff = dOutputBound (SpeedDiff, MaxSpeedChange);
}
- TotalRegValue = (SWORD)((PValue + IValue + DValue)/2);
+ pMD->MotorTargetSpeed += SpeedDiff;
+}
- if (TotalRegValue > MAXIMUM_SPEED_FW)
+/* Absolute position regulation. */
+void dOutputAbsolutePositionRegulation(UBYTE MotorNr)
+{
+ /* Inputs:
+ * - CurrentCaptureCount: current motor position.
+ * - MotorTachoCountToRun: wanted position, filtered with speed and acceleration.
+ *
+ * Outputs:
+ * - MotorActualSpeed: power to be applied to motor.
+ * - MotorOverloaded: set if MotorActualSpeed reached maximum.
+ */
+ SLONG PositionChange;
+ SLONG PositionError;
+ SLONG TotalRegValue;
+
+ MOTORDATA *pMD = &MotorData[MotorNr];
+
+ /* Position update. */
+ if (pMD->MotorMaxSpeed)
{
- TotalRegValue = MAXIMUM_SPEED_FW;
- pMD->MotorOverloaded = 1;
+ dOutputSpeedFilter (MotorNr, pMD->MotorTachoCountToRun - pMD->MotorTachoCountTarget);
+ PositionChange = dOutputFractionalChange (pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR, &pMD->PositionFracError);
+ pMD->MotorTachoCountTarget += PositionChange;
}
- if (TotalRegValue < MAXIMUM_SPEED_RW)
+ else
{
- TotalRegValue = MAXIMUM_SPEED_RW;
- pMD->MotorOverloaded = 1;
+ pMD->MotorTachoCountTarget = pMD->MotorTachoCountToRun;
}
+
+ /* Regulation. */
+ PositionError = pMD->MotorTachoCountTarget - pMD->CurrentCaptureCount;
+ TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError);
+
+ pMD->MotorActualSpeed = TotalRegValue;
+}
+
+/* Regulation function used when Position regulation is enabled */
+/* The regulation form only control one motor at a time */
+void dOutputCalculateMotorPosition(UBYTE MotorNr)
+{
+ SLONG PositionError;
+ SLONG TotalRegValue;
+ SLONG PositionChange;
+
+ MOTORDATA * pMD = &(MotorData[MotorNr]);
+
+ PositionChange = dOutputFractionalChange (pMD->MotorTargetSpeed * MAX_CAPTURE_COUNT / INPUT_SCALE_FACTOR, &pMD->PositionFracError);
+
+ PositionError = (pMD->OldPositionError - pMD->DeltaCaptureCount) + PositionChange;
+
+ TotalRegValue = dOutputPIDRegulation (MotorNr, PositionError);
+
pMD->MotorActualSpeed = (SBYTE)TotalRegValue;
}
@@ -930,11 +1074,11 @@ void dOutputCalculateMotorPosition(UBYTE MotorNr)
void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
{
SLONG TempTurnParameter;
- SWORD PValue;
- SWORD IValue;
- SWORD DValue;
- SWORD CorrectionValue;
- SWORD MotorSpeed;
+ SLONG PValue;
+ SLONG IValue;
+ SLONG DValue;
+ SLONG CorrectionValue;
+ SLONG MotorSpeed;
MOTORDATA * pOne = &(MotorData[MotorOne]);
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
@@ -981,136 +1125,48 @@ 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)(pOne->RegPParameter/REG_CONST_DIV);
-
- DValue = ((SWORD)SyncData.SyncTachoDif - SyncData.SyncOldError) * (SWORD)(pOne->RegDParameter/REG_CONST_DIV);
+ DValue = (SyncData.SyncTachoDif - SyncData.SyncOldError) * (pOne->RegDParameter/REG_CONST_DIV);
SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif;
SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif;
+ SyncData.SyncAccError = dOutputBound (SyncData.SyncAccError, 900);
- if (SyncData.SyncAccError > (SWORD)900)
- {
- SyncData.SyncAccError = 900;
- }
- if (SyncData.SyncAccError < (SWORD)-900)
- {
- SyncData.SyncAccError = -900;
- }
- IValue = SyncData.SyncAccError * (SWORD)(pOne->RegIParameter/REG_CONST_DIV);
+ IValue = SyncData.SyncAccError * (pOne->RegIParameter/REG_CONST_DIV);
- CorrectionValue = (SWORD)((PValue + IValue + DValue)/4);
+ CorrectionValue = (PValue + IValue + DValue) / 4;
MotorSpeed = (SWORD)pOne->MotorTargetSpeed - CorrectionValue;
-
- if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
- {
- MotorSpeed = MAXIMUM_SPEED_FW;
- }
- else
- {
- if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
- {
- MotorSpeed = MAXIMUM_SPEED_RW;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
if (pOne->TurnParameter != 0)
{
if (pOne->MotorTargetSpeed > 0)
{
- if (MotorSpeed > (SWORD)pOne->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pOne->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed < (SWORD)-pOne->MotorTargetSpeed)
- {
- MotorSpeed = -pOne->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, pOne->MotorTargetSpeed);
}
else
{
- if (MotorSpeed < (SWORD)pOne->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pOne->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed > (SWORD)-pOne->MotorTargetSpeed)
- {
- MotorSpeed = -pOne->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, -pOne->MotorTargetSpeed);
}
}
pOne->MotorActualSpeed = (SBYTE)MotorSpeed;
MotorSpeed = (SWORD)pTwo->MotorTargetSpeed + CorrectionValue;
-
- if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
- {
- MotorSpeed = MAXIMUM_SPEED_FW;
- }
- else
- {
- if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
- {
- MotorSpeed = MAXIMUM_SPEED_RW;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, MAXIMUM_SPEED_FW);
if (pOne->TurnParameter != 0)
{
if (pTwo->MotorTargetSpeed > 0)
{
- if (MotorSpeed > (SWORD)pTwo->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pTwo->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed < (SWORD)-pTwo->MotorTargetSpeed)
- {
- MotorSpeed = -pTwo->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, pTwo->MotorTargetSpeed);
}
else
{
- if (MotorSpeed < (SWORD)pTwo->MotorTargetSpeed)
- {
- MotorSpeed = (SWORD)pTwo->MotorTargetSpeed;
- }
- else
- {
- if (MotorSpeed > (SWORD)-pTwo->MotorTargetSpeed)
- {
- MotorSpeed = -pTwo->MotorTargetSpeed;
- }
- }
+ MotorSpeed = dOutputBound (MotorSpeed, -pTwo->MotorTargetSpeed);
}
}
pTwo->MotorActualSpeed = (SBYTE)MotorSpeed;
@@ -1125,7 +1181,8 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
UBYTE MotorOne, MotorTwo;
MotorOne = MotorNr;
MotorTwo = 0xFF;
- for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) {
+ UBYTE i;
+ for(i = MOTOR_A; i <= MOTOR_C; i++) {
if (i == MotorOne)
continue;
if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
@@ -1167,7 +1224,10 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr)
MotorOne = MotorNr;
MotorTwo = 0xFF;
- for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) {
+ // 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) {
@@ -1332,7 +1392,8 @@ void dOutputResetSyncMotors(UBYTE MotorNr)
MotorOne = MotorNr;
MotorTwo = 0xFF;
- for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) {
+ UBYTE i;
+ for(i = MOTOR_A; i <= MOTOR_C; i++) {
if (i == MotorOne)
continue;
if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
@@ -1348,13 +1409,16 @@ void dOutputResetSyncMotors(UBYTE MotorNr)
MOTORDATA * pTwo = &(MotorData[MotorTwo]);
pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
+ pMD->MotorTachoCountTarget = 0;
pTwo->CurrentCaptureCount = 0;
pTwo->MotorTachoCountToRun = 0;
+ pTwo->MotorTachoCountTarget = 0;
}
else
{
pMD->CurrentCaptureCount = 0;
pMD->MotorTachoCountToRun = 0;
+ pMD->MotorTachoCountTarget = 0;
}
}
@@ -1365,7 +1429,8 @@ void dOutputRampDownSynch(UBYTE MotorNr)
MotorOne = MotorNr;
MotorTwo = 0xFF;
- for(UBYTE i = MOTOR_A; i <= MOTOR_C; i++) {
+ UBYTE i;
+ for(i = MOTOR_A; i <= MOTOR_C; i++) {
if (i == MotorOne)
continue;
if (MotorData[i].RegulationMode & REGSTATE_SYNCHRONE) {
@@ -1419,7 +1484,3 @@ void dOutputRampDownSynch(UBYTE MotorNr)
}
}
-void dOutputUpdateRegulationTime(UBYTE rt)
-{
- RegTime = rt;
-}
diff --git a/AT91SAM7S256/Source/d_output.h b/AT91SAM7S256/Source/d_output.h
index ce78246..20955a6 100644
--- a/AT91SAM7S256/Source/d_output.h
+++ b/AT91SAM7S256/Source/d_output.h
@@ -39,10 +39,17 @@
#endif
+#define DEFAULT_MAX_SPEED 80
+#define DEFAULT_MAX_ACCELERATION 20
+
+#define REGULATION_TIME 100 // Measured in 1 mS => 100 mS regulation interval
+//#define REGULATION_TIME 10 // Measured in 1 mS, regulation interval
+
//Constant reffering to RegMode parameter
#define REGSTATE_IDLE 0x00
#define REGSTATE_REGULATED 0x01
#define REGSTATE_SYNCHRONE 0x02
+#define REGSTATE_POSITION 0x04
//Constant reffering to RunState parameter
#define MOTOR_RUN_STATE_IDLE 0x00
@@ -51,9 +58,8 @@
#define MOTOR_RUN_STATE_RAMPDOWN 0x40
#define MOTOR_RUN_STATE_HOLD 0x60
-
-#define RAMP_TIME_INTERVAL 25 // Measured in 1 mS => 25 mS interval
-#define REGULATION_TIME 100 // Measured in 1 mS => 100 mS regulation interval
+// Constants related to Regulation Options
+#define REGOPTION_NO_SATURATION 0x01 // Do not limit intermediary regulation results
enum
{
@@ -76,12 +82,16 @@ void dOutputResetTachoLimit(UBYTE MotorNr);
void dOutputResetBlockTachoLimit(UBYTE MotorNr);
void dOutputResetRotationCaptureCount(UBYTE MotorNr);
void dOutputSetPIDParameters(UBYTE MotorNr, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter);
+void dOutputSetMax(UBYTE MotorNr, SBYTE NewMaxSpeed, SBYTE NewMaxAcceleration);
+void dOutputSetRegulationTime(UBYTE NewRegulationTime);
+void dOutputSetRegulationOptions(UBYTE NewRegulationOptions);
void dOutputRegulateMotor(UBYTE MotorNr);
void dOutputCalculateRampUpParameter(UBYTE MotorNr, ULONG NewTachoLimit);
void dOutputRampDownFunction(UBYTE MotorNr);
void dOutputRampUpFunction(UBYTE MotorNr);
void dOutputTachoLimitControl(UBYTE MotorNr);
+void dOutputAbsolutePositionRegulation(UBYTE MotorNr);
void dOutputCalculateMotorPosition(UBYTE MotorNr);
void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo);
void dOutputMotorReachedTachoLimit(UBYTE MotorNr);