aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-10-09 13:53:55 +0200
committerNicolas Schodet2010-11-01 10:33:33 +0100
commit2c9f8347499be7453b8ce4a6f11eb5b32b94b780 (patch)
treee3ee53fc01297080c407778103b49bdf03151561
parent3b2647cd33fe16f372f1c3c99d16282ea0b74bbd (diff)
add absolute position control
-rw-r--r--AT91SAM7S256/Source/c_output.c10
-rw-r--r--AT91SAM7S256/Source/c_output.iom7
-rw-r--r--AT91SAM7S256/Source/d_output.c76
-rw-r--r--AT91SAM7S256/Source/d_output.h2
4 files changed, 85 insertions, 10 deletions
diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c
index 9566938..ff27ee8 100644
--- a/AT91SAM7S256/Source/c_output.c
+++ b/AT91SAM7S256/Source/c_output.c
@@ -90,11 +90,6 @@ void cOutputCtrl(void)
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;
@@ -125,6 +120,11 @@ void cOutputCtrl(void)
dOutputDisableRegulation(Tmp);
}
}
+ 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_PID_VALUES)
{
IOMapOutput.Outputs[Tmp].Flags &= ~UPDATE_PID_VALUES;
diff --git a/AT91SAM7S256/Source/c_output.iom b/AT91SAM7S256/Source/c_output.iom
index 80e35de..df5137b 100644
--- a/AT91SAM7S256/Source/c_output.iom
+++ b/AT91SAM7S256/Source/c_output.iom
@@ -49,9 +49,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
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index d953b84..978be66 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -35,6 +35,13 @@
#define COAST_MOTOR_MODE 0
+#define BOUND(var, minmax) do { \
+ if ((var) > (minmax)) \
+ (var) = (minmax); \
+ else if ((var) < -(minmax)) \
+ (var) = -(minmax); \
+} while (0)
+
void dOutputRampDownSynch(UBYTE MotorNr);
typedef struct
@@ -271,7 +278,12 @@ void dOutputSetPIDParameters(UBYTE Motor, UBYTE NewRegPParameter, UBYTE NewRegIP
/* TachoCountToRun is calculated as a signed value */
void dOutputSetTachoLimit(UBYTE MotorNr, ULONG BlockTachoCntToTravel)
{
- if (BlockTachoCntToTravel == 0)
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_POSITION)
+ {
+ MotorData[MotorNr].MotorRunForever = 0;
+ MotorData[MotorNr].MotorTachoCountToRun = BlockTachoCntToTravel;
+ }
+ else if (BlockTachoCntToTravel == 0)
{
MotorData[MotorNr].MotorRunForever = 1;
}
@@ -651,6 +663,11 @@ void dOutputRampDownFunction(UBYTE MotorNr)
/* Function used to tell whether the wanted position is obtained */
void dOutputTachoLimitControl(UBYTE MotorNr)
{
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_POSITION)
+ {
+ /* No limit when doing absolute position regulation. */
+ return;
+ }
if (MotorData[MotorNr].MotorRunForever == 0)
{
if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
@@ -745,7 +762,11 @@ void dOutputRegulateMotor(UBYTE MotorNr)
UBYTE SyncMotorOne;
UBYTE SyncMotorTwo;
- if (MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)
+ if (MotorData[MotorNr].RegulationMode & REGSTATE_POSITION)
+ {
+ dOutputAbsolutePositionRegulation(MotorNr);
+ }
+ else if (MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)
{
dOutputCalculateMotorPosition(MotorNr);
}
@@ -763,6 +784,57 @@ void dOutputRegulateMotor(UBYTE MotorNr)
}
}
+/* Absolute position regulation. */
+void dOutputAbsolutePositionRegulation(UBYTE MotorNr)
+{
+ /* Inputs:
+ * - CurrentCaptureCount: current motor position.
+ * - MotorTachoCountToRun: wanted position.
+ *
+ * - RegPParameter, RegIParameter, RegDParameter: PID parameters.
+ *
+ * Outputs:
+ * - MotorActualSpeed: power to be applied to motor.
+ * - MotorOverloaded: set if MotorActualSpeed reached maximum.
+ *
+ * - OldPositionError: remember last error, used for D part.
+ * - AccError: Accumulated error, used for I part.
+ */
+ int PositionError;
+ int PValue, DValue, IValue, TotalRegValue;
+
+ MOTORDATA *pMD = &MotorData[MotorNr];
+
+ PositionError = pMD->MotorTachoCountToRun - pMD->CurrentCaptureCount;
+ BOUND (PositionError, 32000);
+
+ PValue = PositionError * pMD->RegPParameter / REG_CONST_DIV;
+ BOUND (PValue, REG_MAX_VALUE);
+
+ DValue = (PositionError - pMD->OldPositionError) * pMD->RegDParameter / REG_CONST_DIV;
+ pMD->OldPositionError = PositionError;
+
+ pMD->AccError = (pMD->AccError * 3 + PositionError) / 4;
+ BOUND (pMD->AccError, 800);
+
+ IValue = pMD->AccError * pMD->RegIParameter / REG_CONST_DIV;
+ BOUND (IValue, REG_MAX_VALUE);
+
+ TotalRegValue = (PValue + IValue + DValue) / 2;
+ if (TotalRegValue > MAXIMUM_SPEED_FW)
+ {
+ TotalRegValue = MAXIMUM_SPEED_FW;
+ pMD->MotorOverloaded = 1;
+ }
+ else if (TotalRegValue < MAXIMUM_SPEED_RW)
+ {
+ TotalRegValue = MAXIMUM_SPEED_RW;
+ pMD->MotorOverloaded = 1;
+ }
+
+ 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)
diff --git a/AT91SAM7S256/Source/d_output.h b/AT91SAM7S256/Source/d_output.h
index 7369b34..6b944a8 100644
--- a/AT91SAM7S256/Source/d_output.h
+++ b/AT91SAM7S256/Source/d_output.h
@@ -43,6 +43,7 @@
#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
@@ -79,6 +80,7 @@ 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);