d_output.c

乐高机器人的源码,开发平台是IAR_for_AVR.

//
// Date init       14.12.2004
//
// Revision date   $Date:: 16-05-06 12:13                                    $
//
// Filename        $Workfile:: d_output.c                                    $
//
// Version         $Revision:: 117                                           $
//
// Archive         $Archive:: /LMS2006/Sys01/Main/Firmware/Source/d_output.c $
//
// Platform        C
//

#include  "stdconst.h"
#include  "m_sched.h"
#include  "d_output.h"
#include  "d_output.r"

#define MAXIMUM_SPEED_FW         100
#define MAXIMUM_SPEED_RW         -100

#define INPUT_SCALE_FACTOR       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 REGULATION_TIME          100          // Measured in 1 mS => 100 mS regulation interval

#define RAMPDOWN_STATE_RAMPDOWN  0
#define RAMPDOWN_STATE_CONTINIUE 1

#define COAST_MOTOR_MODE         0

void dOutputRampDownSynch(UBYTE MotorNr);

typedef struct
{
  SBYTE MotorSetSpeed;                        // Motor setpoint in speed
  SBYTE MotorTargetSpeed;                     // Speed order for the movement
  SBYTE MotorActualSpeed;                     // Actual speed for motor (Calculated within the PID regulation)
  SBYTE TurnParameter;                        // Tell the turning parameter used
  UBYTE RegPParameter;                        // Current P parameter used within the regulation
  UBYTE RegIParameter;                        // Current I parameter used within the regulation
  UBYTE RegDParameter;                        // Current D parameter used within the regulation
  UBYTE RegulationTimeCount;                  // Time counter used to evaluate when the regulation should run again (100 mS)
  UBYTE MotorRunState;                        // Hold current motor state (Ramp-up, Running, Ramp-Down, Idle)
  UBYTE RegulationMode;                       // Hold current regulation mode (Position control, Synchronization mode)
  UBYTE MotorOverloaded;                      // Set if the motor speed in regulation is calculated to be above maximum
  UBYTE MotorRunForever;                      // Tell that the motor is set to run forever
  UWORD MotorRampDownCount;                   // Counter to tell if the ramp-down can reach it gaol and therefor need some additional help
  SWORD MotorRampDownIncrement;               // Tell the number of count between each speed adjustment during Ramp-Down
  UWORD MotorRampUpCount;                     // Used to speedup Ramp-Up if position regulation is not enabled
  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
  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
  SLONG MotorBlockTachoCount;                 // Hold CaptureCount for current movement
  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
}MOTORDATA;

typedef struct
{
  SLONG SyncTachoDif;
  SLONG SyncTurnParameter;
  SWORD SyncOldError;
  SWORD SyncAccError;
}SYNCMOTORDATA;

static    MOTORDATA         MotorData[3];
static    SYNCMOTORDATA     SyncData;

void      dOutputInit(void)
{
  UBYTE Temp;

  OUTPUTInit;
  ENABLECaptureMotorA;
  ENABLECaptureMotorB;
  ENABLECaptureMotorC;

  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;
    INSERTMode(Temp, COAST_MOTOR_MODE);
    INSERTSpeed(Temp, MotorData[Temp].MotorSetSpeed);
  }
}

/* This function is called every 1 mS and will go through all the motors and there dependencies */
/* Actual motor speed is only passed (updated) to the AVR controller form this function */
/* DeltacaptureCount used to count number of Tachocount within last 100 mS. Used with position control regulation */
/* CurrentCaptureCount used to tell total current position. Used to tell when movement has been obtained */
/* MotorBlockTachoCount tell current position within current movement. Reset when a new block is started from the VM */
/* RotationCaptureCount is additional counter for the rotationsensor. Uses it own value so it does conflict with other CaptureCount */
void dOutputCtrl(void)
{
  UBYTE MotorNr;
  SLONG NewTachoCount[3];

  TACHOCaptureReadResetAll(NewTachoCount[MOTOR_A], NewTachoCount[MOTOR_B], NewTachoCount[MOTOR_C]);

  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++;

    if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RAMPUP)
    {
      dOutputRampUpFunction(MotorNr);
    }
    if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RAMPDOWN)
    {
      dOutputRampDownFunction(MotorNr);
    }
    if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_RUNNING)
    {
      dOutputTachoLimitControl(MotorNr);
    }
    if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_IDLE)
    {
      dOutputMotorIdleControl(MotorNr);
    }
    if (MotorData[MotorNr].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;

    }
    if (MotorData[MotorNr].RegulationTimeCount > REGULATION_TIME)
    {
      MotorData[MotorNr].RegulationTimeCount = 0;
      dOutputRegulateMotor(MotorNr);
      MotorData[MotorNr].DeltaCaptureCount = 0;
    }
  }
  INSERTSpeed(MOTOR_A, MotorData[MOTOR_A].MotorActualSpeed);
  INSERTSpeed(MOTOR_B, MotorData[MOTOR_B].MotorActualSpeed);
  INSERTSpeed(MOTOR_C, MotorData[MOTOR_C].MotorActualSpeed);
}

void      dOutputExit(void)
{
  OUTPUTExit;
}

/* Called eveyr 1 mS */
/* Data mapping for controller (IO-Map is updated with these values) */
void dOutputGetMotorParameters(UBYTE *CurrentMotorSpeed, SLONG *TachoCount, SLONG *BlockTachoCount, UBYTE *RunState, UBYTE *MotorOverloaded, SLONG *RotationCount)
{
  UBYTE Tmp;

  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;
  }
}

void dOutputSetMode(UBYTE Motor, UBYTE Mode)     //Set motor mode (break, Float)
{
  INSERTMode(Motor, Mode);
}

/* Update the regulation state for the motor */
/* Need to reset regulation parameter depending on current status of the motor */
/* AccError & OldPositionError used for position regulation and Sync Parameter are used for synchronization regulation */
void dOutputEnableRegulation(UBYTE MotorNr, UBYTE RegulationMode)
{
  MotorData[MotorNr].RegulationMode = RegulationMode;

  if ((MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED) && (MotorData[MotorNr].MotorSetSpeed == 0) && (MotorData[MotorNr].MotorRunState != MOTOR_RUN_STATE_RAMPDOWN))
  {
    MotorData[MotorNr].AccError = 0;
    MotorData[MotorNr].OldPositionError = 0;
  }

  if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
  {
    if (((MotorData[MotorNr].MotorActualSpeed == 0) || (MotorData[MotorNr].TurnParameter != 0) || (MotorData[MotorNr].TurnParameter == 0)) && (MotorData[MotorNr].MotorRunState != MOTOR_RUN_STATE_RAMPDOWN))
    {
      SyncData.SyncTachoDif = 0;

      SyncData.SyncAccError = 0;
      SyncData.SyncOldError = 0;
      SyncData.SyncTurnParameter = 0;
    }
  }
}

/* Disable current regulation if enabled */
void dOutputDisableRegulation(UBYTE MotorNr)
{
  MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
}

/* Calling this function with reset count which tell current position and which is used to tell if the wanted position is obtained */
/* 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;

  if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
  {
    dOutputResetSyncMotors(MotorNr);
  }

  if (MotorData[MotorNr].MotorRunForever == 1)
  {
    MotorData[MotorNr].MotorRunForever = 0;                   // To ensure that we get the same functionality for all combination on motor durations
  }
}

/* MotorBlockTachoCount tells current position in current movement. */
/* Used within the synchronization to compare current motor position. Reset on every new movement from the VM */
void dOutputResetBlockTachoLimit(UBYTE MotorNr)
{
  MotorData[MotorNr].MotorBlockTachoCount = 0;
}

/* Additional counter add to help the VM application keep track of number of rotation for the rotation sensor */
/* This values can be reset independtly from the other tacho count values used with regulation and position control */
void dOutputResetRotationCaptureCount(UBYTE MotorNr)
{
  MotorData[MotorNr].RotationCaptureCount = 0;
}

/* Can be used to set new PID values */
void dOutputSetPIDParameters(UBYTE Motor, UBYTE NewRegPParameter, UBYTE NewRegIParameter, UBYTE NewRegDParameter)
{
  MotorData[Motor].RegPParameter = NewRegPParameter;
  MotorData[Motor].RegIParameter = NewRegIParameter;
  MotorData[Motor].RegDParameter = NewRegDParameter;
}

/* 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)
{
  if (BlockTachoCntToTravel == 0)
  {
    MotorData[MotorNr].MotorRunForever = 1;
  }
  else
  {
    MotorData[MotorNr].MotorRunForever = 0;

    if (MotorData[MotorNr].MotorSetSpeed == 0)
    {
      if (MotorData[MotorNr].MotorTargetSpeed > 0)
      {
        MotorData[MotorNr].MotorTachoCountToRun += BlockTachoCntToTravel;
      }
      else
      {
        MotorData[MotorNr].MotorTachoCountToRun -= BlockTachoCntToTravel;
      }
    }
    else
    {
      if (MotorData[MotorNr].MotorSetSpeed > 0)
      {
        MotorData[MotorNr].MotorTachoCountToRun += BlockTachoCntToTravel;
      }
      else
      {
        MotorData[MotorNr].MotorTachoCountToRun -= 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))
  {
    if (MotorData[MotorNr].MotorTargetSpeed == 0)
    {
      MotorData[MotorNr].AccError = 0;
      MotorData[MotorNr].OldPositionError = 0;
      MotorData[MotorNr].RegulationTimeCount = 0;
      MotorData[MotorNr].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;
      }
      break;

      case MOTOR_RUN_STATE_RAMPUP:
      {
        if (MotorData[MotorNr].MotorSetSpeed == 0)
        {
          MotorData[MotorNr].MotorSetSpeed = Speed;
          MotorData[MotorNr].TurnParameter = NewTurnParameter;
          MotorData[MotorNr].MotorRampUpIncrement = 0;
          MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
          MotorData[MotorNr].MotorRampUpCount = 0;
        }
        else
        {
          if (Speed > 0)
          {
            if (MotorData[MotorNr].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;
            }
          }
          else
          {
            if (MotorData[MotorNr].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;
            }
          }
        }
      }
      break;

      case MOTOR_RUN_STATE_RUNNING:
      {
        MotorData[MotorNr].MotorSetSpeed = Speed;
        MotorData[MotorNr].MotorTargetSpeed = Speed;
        MotorData[MotorNr].TurnParameter = NewTurnParameter;

        if (MotorData[MotorNr].MotorSetSpeed == 0)
        {
          NewMotorRunState = MOTOR_RUN_STATE_HOLD;
        }
      }
      break;

      case MOTOR_RUN_STATE_RAMPDOWN:
      {
        if (MotorData[MotorNr].MotorTargetSpeed >= 0)
        {
          if (MotorData[MotorNr].MotorSetSpeed <= Speed)
          {
            NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
          }
          else
          {
            MotorData[MotorNr].MotorSetSpeed = Speed;
            MotorData[MotorNr].TurnParameter = NewTurnParameter;
            MotorData[MotorNr].MotorRampDownIncrement = 0;