⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 d_output.c

📁 乐高机器人的源码,开发平台是IAR_for_AVR.
💻 C
📖 第 1 页 / 共 4 页
字号:
🔍
1234 
  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;
  }

  DValue = (PositionError - MotorData[MotorNr].OldPositionError) * (SWORD)(MotorData[MotorNr].RegDParameter/REG_CONST_DIV);
  MotorData[MotorNr].OldPositionError = PositionError;

  MotorData[MotorNr].AccError = (MotorData[MotorNr].AccError * 3) + PositionError;
  MotorData[MotorNr].AccError = MotorData[MotorNr].AccError / 4;

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

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

  if (TotalRegValue > MAXIMUM_SPEED_FW)
  {
    TotalRegValue = MAXIMUM_SPEED_FW;
    MotorData[MotorNr].MotorOverloaded = 1;
  }
  if (TotalRegValue < MAXIMUM_SPEED_RW)
  {
    TotalRegValue = MAXIMUM_SPEED_RW;
    MotorData[MotorNr].MotorOverloaded = 1;
  }
  MotorData[MotorNr].MotorActualSpeed = (SBYTE)TotalRegValue;
}

/* Regulation function used when syncrhonization regulation is enabled */
/* The regulation form controls two motors at a time */
void dOutputSyncMotorPosition(UBYTE MotorOne, UBYTE MotorTwo)
{
  SLONG TempTurnParameter;
  SWORD PValue;
  SWORD IValue;
  SWORD DValue;
  SWORD CorrectionValue;
  SWORD MotorSpeed;

  SyncData.SyncTachoDif = (SLONG)((MotorData[MotorOne].MotorBlockTachoCount) - (MotorData[MotorTwo].MotorBlockTachoCount));

  if (MotorData[MotorOne].TurnParameter != 0)
  {
    if ((MotorData[MotorOne].MotorBlockTachoCount != 0) || (MotorData[MotorTwo].MotorBlockTachoCount))
    {
      if (MotorData[MotorOne].MotorTargetSpeed >= 0)
      {
        if (MotorData[MotorOne].TurnParameter > 0)
        {
          TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorTwo].TurnParameter * (SLONG)MotorData[MotorTwo].MotorTargetSpeed)/100);
        }
        else
        {
          TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorOne].TurnParameter * (SLONG)MotorData[MotorOne].MotorTargetSpeed)/100);
        }
      }
      else
      {
        if (MotorData[MotorOne].TurnParameter > 0)
        {
          TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorOne].TurnParameter * (-(SLONG)MotorData[MotorOne].MotorTargetSpeed))/100);
        }
        else
        {
          TempTurnParameter = (SLONG)(((SLONG)MotorData[MotorTwo].TurnParameter * (-(SLONG)MotorData[MotorTwo].MotorTargetSpeed))/100);
        }
      }
    }
    else
    {
      TempTurnParameter = MotorData[MotorOne].TurnParameter;
    }
  }
  else
  {
    TempTurnParameter = 0;
  }

  SyncData.SyncTurnParameter += (SLONG)(((TempTurnParameter * (MAX_CAPTURE_COUNT))/INPUT_SCALE_FACTOR)*2);
  //SyncTurnParameter should ophold difference between the two motors.

  SyncData.SyncTachoDif += SyncData.SyncTurnParameter;

  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);
  SyncData.SyncOldError = (SWORD)SyncData.SyncTachoDif;

  SyncData.SyncAccError += (SWORD)SyncData.SyncTachoDif;

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

  MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed - CorrectionValue;

  if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
  {
    MotorSpeed = MAXIMUM_SPEED_FW;
  }
  else
  {
    if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
    {
      MotorSpeed = MAXIMUM_SPEED_RW;
    }
  }

  if (MotorData[MotorOne].TurnParameter != 0)
  {
    if (MotorData[MotorOne].MotorTargetSpeed > 0)
    {
      if (MotorSpeed > (SWORD)MotorData[MotorOne].MotorTargetSpeed)
      {
        MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed;
      }
      else
      {
        if (MotorSpeed < (SWORD)-MotorData[MotorOne].MotorTargetSpeed)
        {
          MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed;
        }
      }
    }
    else
    {
      if (MotorSpeed < (SWORD)MotorData[MotorOne].MotorTargetSpeed)
      {
        MotorSpeed = (SWORD)MotorData[MotorOne].MotorTargetSpeed;
      }
      else
      {
        if (MotorSpeed > (SWORD)-MotorData[MotorOne].MotorTargetSpeed)
        {
          MotorSpeed = -MotorData[MotorOne].MotorTargetSpeed;
        }
      }
    }
  }
  MotorData[MotorOne].MotorActualSpeed = (SBYTE)MotorSpeed;

  MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed + CorrectionValue;

  if (MotorSpeed > (SWORD)MAXIMUM_SPEED_FW)
  {
    MotorSpeed = MAXIMUM_SPEED_FW;
  }
  else
  {
    if (MotorSpeed < (SWORD)MAXIMUM_SPEED_RW)
    {
      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;
        }
      }
    }
    else
    {
      if (MotorSpeed < (SWORD)MotorData[MotorTwo].MotorTargetSpeed)
      {
        MotorSpeed = (SWORD)MotorData[MotorTwo].MotorTargetSpeed;
      }
      else
      {
        if (MotorSpeed > (SWORD)-MotorData[MotorTwo].MotorTargetSpeed)
        {
          MotorSpeed = -MotorData[MotorTwo].MotorTargetSpeed;
        }
      }
    }
  }
  MotorData[MotorTwo].MotorActualSpeed = (SBYTE)MotorSpeed;
}

//Called when the motor is ramping down
void dOutputMotorReachedTachoLimit(UBYTE MotorNr)
{
  UBYTE MotorOne, MotorTwo;

  if (MotorData[MotorNr].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;
      }
    }
  }
  else
  {
    if (MotorData[MotorNr].MotorSetSpeed == 0)
    {
      MotorData[MotorNr].MotorSetSpeed = 0;
	    MotorData[MotorNr].MotorTargetSpeed = 0;
      MotorData[MotorNr].MotorActualSpeed = 0;
    }
    MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
    MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
  }
}

/* Function used for control tacho limit when motors are synchronised */
/* Special control is needed when the motor are turning */
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
1234

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -