summaryrefslogtreecommitdiff
path: root/AT91SAM7S256/Source/d_output.c
diff options
context:
space:
mode:
authorJohn Hansen2010-10-16 18:12:46 +0000
committerNicolas Schodet2011-07-04 00:37:03 +0200
commit565424ed7a688f910f63fc1ce82e39ebf0dfb28f (patch)
treeb0debc1bc66133151654a885b96414e382b62b93 /AT91SAM7S256/Source/d_output.c
parentfb5f5e4551230270344616648c78cc4896f6626c (diff)
NBCNXC_131
git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc_128/trunk@21 c9361245-7fe8-9947-84e8-057757c4e366
Diffstat (limited to 'AT91SAM7S256/Source/d_output.c')
-rw-r--r--AT91SAM7S256/Source/d_output.c366
1 files changed, 11 insertions, 355 deletions
diff --git a/AT91SAM7S256/Source/d_output.c b/AT91SAM7S256/Source/d_output.c
index 843db57..c3fdeed 100644
--- a/AT91SAM7S256/Source/d_output.c
+++ b/AT91SAM7S256/Source/d_output.c
@@ -963,6 +963,16 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
SyncData.SyncTachoDif += SyncData.SyncTurnParameter;
+ if (SyncData.SyncTachoDif > 500)
+ {
+ SyncData.SyncTachoDif = 500;
+ }
+ if (SyncData.SyncTachoDif < -500)
+ {
+ SyncData.SyncTachoDif = -500;
+ }
+
+ /*
if ((SWORD)SyncData.SyncTachoDif > 500)
{
SyncData.SyncTachoDif = 500;
@@ -971,6 +981,7 @@ void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
{
SyncData.SyncTachoDif = -500;
}
+ */
PValue = (SWORD)SyncData.SyncTachoDif * (SWORD)(pOne->RegPParameter/REG_CONST_DIV);
@@ -1116,140 +1127,6 @@ void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
pTwo->MotorRunState = pTwo->RunStateAtLimit;
pTwo->RegulationMode = REGSTATE_IDLE;
}
-/*
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorOne].RegulationMode = REGSTATE_IDLE;
- MotorData[MotorTwo].MotorSetSpeed = 0;
- MotorData[MotorTwo].MotorTargetSpeed = 0;
- MotorData[MotorTwo].MotorActualSpeed = 0;
- MotorData[MotorTwo].MotorRunState = pTwo->RunStateAtLimit;
- 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 = pOne->RunStateAtLimit;
- MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
- }
- }
-*/
}
else
{
@@ -1281,83 +1158,6 @@ void dOutputSyncTachoLimitControl(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
-/*
- 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;
- }
- }
- }
-*/
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pOne = &(MotorData[MotorOne]);
@@ -1523,77 +1323,6 @@ void dOutputResetSyncMotors(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
-/*
- 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;
- }
- }
- }
-*/
MOTORDATA * pMD = &(MotorData[MotorNr]);
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
@@ -1627,79 +1356,6 @@ void dOutputRampDownSynch(UBYTE MotorNr)
}
if (MotorTwo == 0xFF)
MotorOne = 0xFF;
-/*
- 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;
- }
- }
- }
-*/
if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
{
MOTORDATA * pOne = &(MotorData[MotorOne]);