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

📄 d_output.c

📁 乐高机器人的源码,开发平台是IAR_for_AVR.
💻 C
📖 第 1 页 / 共 4 页
字号:
🔍
1234 
            MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
            MotorData[MotorNr].MotorRampDownCount = 0;
          }
        }
        else
        {
          if (MotorData[MotorNr].MotorSetSpeed >= Speed)
          {
            NewMotorRunState = MOTOR_RUN_STATE_RUNNING;
          }
          else
          {
            MotorData[MotorNr].MotorSetSpeed = Speed;
            MotorData[MotorNr].TurnParameter = NewTurnParameter;
            MotorData[MotorNr].MotorRampDownIncrement = 0;
            MotorData[MotorNr].MotorRampTachoCountStart = MotorData[MotorNr].CurrentCaptureCount;
            MotorData[MotorNr].MotorRampDownCount = 0;
          }
        }
      }
      break;
    }
    MotorData[MotorNr].MotorRunState = NewMotorRunState;
    MotorData[MotorNr].MotorOverloaded = 0;
  }
}

/* Function used for controlling the Ramp-up periode */
/* Ramp-up is done with 1 increment in speed every X number of TachoCount, where X depend on duration of the periode and the wanted speed */
void dOutputRampUpFunction(UBYTE MotorNr)
{
  if (MotorData[MotorNr].MotorTargetSpeed == 0)
  {
    if (MotorData[MotorNr].MotorSetSpeed > 0)
    {
      MotorData[MotorNr].MotorTargetSpeed = MIN_MOVEMENT_POWER;
    }
    else
    {
      MotorData[MotorNr].MotorTargetSpeed = -MIN_MOVEMENT_POWER;
    }
  }
  else
  {
    if (MotorData[MotorNr].MotorRampUpIncrement == 0)
    {
      if (MotorData[MotorNr].MotorSetSpeed > 0)
      {
        MotorData[MotorNr].MotorRampUpIncrement = (SWORD)((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart) / (MotorData[MotorNr].MotorSetSpeed - MotorData[MotorNr].MotorTargetSpeed));
      }
      else
      {
        MotorData[MotorNr].MotorRampUpIncrement = (SWORD)(-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart) / (MotorData[MotorNr].MotorSetSpeed - MotorData[MotorNr].MotorTargetSpeed)));
      }
      MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
    }
    if (MotorData[MotorNr].MotorSetSpeed > 0)
    {
      if (MotorData[MotorNr].CurrentCaptureCount > (MotorData[MotorNr].MotorRampTachoCountOld + MotorData[MotorNr].MotorRampUpIncrement))
      {
        MotorData[MotorNr].MotorTargetSpeed += 1;
        MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
        MotorData[MotorNr].MotorRampUpCount = 0;
      }
      else
      {
        if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
        {
          MotorData[MotorNr].MotorRampUpCount++;
          if (MotorData[MotorNr].MotorRampUpCount > 100)
          {
            MotorData[MotorNr].MotorRampUpCount = 0;
            MotorData[MotorNr].MotorTargetSpeed++;
          }
        }
      }
    }
    else
    {
      if (MotorData[MotorNr].CurrentCaptureCount < (MotorData[MotorNr].MotorRampTachoCountOld + MotorData[MotorNr].MotorRampUpIncrement))
      {
        MotorData[MotorNr].MotorTargetSpeed -= 1;
        MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
        MotorData[MotorNr].MotorRampUpCount = 0;
      }
      else
      {
        if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
        {
          MotorData[MotorNr].MotorRampUpCount++;
          if (MotorData[MotorNr].MotorRampUpCount > 100)
          {
            MotorData[MotorNr].MotorRampUpCount = 0;
            MotorData[MotorNr].MotorTargetSpeed--;
          }
        }
      }
    }
  }
  if (MotorData[MotorNr].MotorSetSpeed > 0)
  {
    if ((MotorData[MotorNr].CurrentCaptureCount - MotorData[MotorNr].MotorRampTachoCountStart) >= (MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].MotorRampTachoCountStart))
    {
      MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
      MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;	
    }
  }
  else
  {
    if ((MotorData[MotorNr].CurrentCaptureCount + MotorData[MotorNr].MotorRampTachoCountStart) <= (MotorData[MotorNr].MotorTachoCountToRun + MotorData[MotorNr].MotorRampTachoCountStart))
    {
      MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
      MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;	
    }
  }
  if (MotorData[MotorNr].MotorSetSpeed > 0)
  {
    if (MotorData[MotorNr].MotorTargetSpeed > MotorData[MotorNr].MotorSetSpeed)
    {
      MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
    }
  }
  else
  {
    if (MotorData[MotorNr].MotorTargetSpeed < MotorData[MotorNr].MotorSetSpeed)
    {
      MotorData[MotorNr].MotorTargetSpeed = MotorData[MotorNr].MotorSetSpeed;
    }
  }
  if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE)
  {
    MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed;
  }
}

/* Function used for controlling the Ramp-down periode */
/* Ramp-down is done with 1 decrement in speed every X number of TachoCount, where X depend on duration of the periode and the wanted speed */
void dOutputRampDownFunction(UBYTE MotorNr)
{
  if (MotorData[MotorNr].MotorRampDownIncrement == 0)
  {
    if (MotorData[MotorNr].MotorTargetSpeed > 0)
    {
      if ((MotorData[MotorNr].MotorTargetSpeed > MIN_MOVEMENT_POWER) && (MotorData[MotorNr].MotorSetSpeed == 0))
      {
        MotorData[MotorNr].MotorRampDownIncrement = ((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / ((MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed) - MIN_MOVEMENT_POWER));
      }
      else
      {
        MotorData[MotorNr].MotorRampDownIncrement = ((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / (MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed));
      }
    }
    else
    {
      if ((MotorData[MotorNr].MotorTargetSpeed < -MIN_MOVEMENT_POWER) && (MotorData[MotorNr].MotorSetSpeed == 0))
      {
        MotorData[MotorNr].MotorRampDownIncrement = (-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / ((MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed) + MIN_MOVEMENT_POWER)));
      }
      else
      {
        MotorData[MotorNr].MotorRampDownIncrement = (-((MotorData[MotorNr].MotorTachoCountToRun - MotorData[MotorNr].CurrentCaptureCount) / (MotorData[MotorNr].MotorTargetSpeed - MotorData[MotorNr].MotorSetSpeed)));
      }
    }
    MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
  }
  if (MotorData[MotorNr].MotorTargetSpeed > 0)
  {
    if (MotorData[MotorNr].CurrentCaptureCount > (MotorData[MotorNr].MotorRampTachoCountOld + (SLONG)MotorData[MotorNr].MotorRampDownIncrement))
    {
      MotorData[MotorNr].MotorTargetSpeed--;
      if (MotorData[MotorNr].MotorTargetSpeed < MIN_MOVEMENT_POWER)
      {
        MotorData[MotorNr].MotorTargetSpeed = MIN_MOVEMENT_POWER;
      }
      MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
      MotorData[MotorNr].MotorRampDownCount = 0;
      dOutputRampDownSynch(MotorNr);
    }
    else
    {
      if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
      {
        MotorData[MotorNr].MotorRampDownCount++;
        if (MotorData[MotorNr].MotorRampDownCount > (UWORD)(30 * MotorData[MotorNr].MotorRampDownIncrement))
        {
          MotorData[MotorNr].MotorRampDownCount = (UWORD)(20 * MotorData[MotorNr].MotorRampDownIncrement);
          MotorData[MotorNr].MotorTargetSpeed++;
        }
      }
    }
  }
  else
  {
    if (MotorData[MotorNr].CurrentCaptureCount < (MotorData[MotorNr].MotorRampTachoCountOld + (SLONG)MotorData[MotorNr].MotorRampDownIncrement))
    {
      MotorData[MotorNr].MotorTargetSpeed++;
      if (MotorData[MotorNr].MotorTargetSpeed > -MIN_MOVEMENT_POWER)
      {
        MotorData[MotorNr].MotorTargetSpeed = -MIN_MOVEMENT_POWER;
      }
      MotorData[MotorNr].MotorRampTachoCountOld = MotorData[MotorNr].CurrentCaptureCount;
      MotorData[MotorNr].MotorRampDownCount = 0;
      dOutputRampDownSynch(MotorNr);
    }
    else
    {
      if (!(MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED))
      {
        MotorData[MotorNr].MotorRampDownCount++;
        if (MotorData[MotorNr].MotorRampDownCount > (UWORD)(30 * (-MotorData[MotorNr].MotorRampDownIncrement)))
        {
          MotorData[MotorNr].MotorRampDownCount = (UWORD)(20 * (-MotorData[MotorNr].MotorRampDownIncrement));
          MotorData[MotorNr].MotorTargetSpeed--;
        }
      }
    }
  }
  if ((MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE) && (MotorData[MotorNr].TurnParameter != 0))
  {
    dOutputSyncTachoLimitControl(MotorNr);
    if (MotorData[MotorNr].MotorRunState == MOTOR_RUN_STATE_IDLE)
    {
      dOutputMotorReachedTachoLimit(MotorNr);
    }
  }
  else
  {
    if (MotorData[MotorNr].MotorTargetSpeed > 0)
    {
      if (MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun)
      {
        dOutputMotorReachedTachoLimit(MotorNr);
      }
    }
    else
    {
      if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun)
      {
        dOutputMotorReachedTachoLimit(MotorNr);
      }
    }
  }
  if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE)
  {
    MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed;
  }
}

/* Function used to tell whether the wanted position is obtained */
void dOutputTachoLimitControl(UBYTE MotorNr)
{
  if (MotorData[MotorNr].MotorRunForever == 0)
  {
    if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
    {
      dOutputSyncTachoLimitControl(MotorNr);
    }
    else
    {
      if (MotorData[MotorNr].MotorSetSpeed > 0)
      {
        if ((MotorData[MotorNr].CurrentCaptureCount >= MotorData[MotorNr].MotorTachoCountToRun))
        {
          MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
          MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
        }
      }
      else
      {
        if (MotorData[MotorNr].MotorSetSpeed < 0)
        {
          if (MotorData[MotorNr].CurrentCaptureCount <= MotorData[MotorNr].MotorTachoCountToRun)
          {
            MotorData[MotorNr].MotorRunState = MOTOR_RUN_STATE_IDLE;
            MotorData[MotorNr].RegulationMode = REGSTATE_IDLE;
          }
        }
      }
    }
  }
  else
  {
    if (MotorData[MotorNr].CurrentCaptureCount > MAX_COUNT_TO_RUN)
    {
      MotorData[MotorNr].CurrentCaptureCount = 0;
    }
    if (MotorData[MotorNr].MotorTargetSpeed != 0)
    {
      MotorData[MotorNr].MotorTachoCountToRun = MotorData[MotorNr].CurrentCaptureCount;
    }
  }
  if (MotorData[MotorNr].RegulationMode == REGSTATE_IDLE)
  {
    MotorData[MotorNr].MotorActualSpeed = MotorData[MotorNr].MotorTargetSpeed;
  }
}

/* Function used to decrease speed slowly when the motor is set to idle */
void dOutputMotorIdleControl(UBYTE MotorNr)
{
  INSERTMode(MotorNr, COAST_MOTOR_MODE);

  if (MotorData[MotorNr].MotorActualSpeed != 0)
  {
    if (MotorData[MotorNr].MotorActualSpeed > 0)
    {
      MotorData[MotorNr].MotorActualSpeed--;
    }
    else
    {
      MotorData[MotorNr].MotorActualSpeed++;
    }
  }

  if (MotorData[MotorNr].MotorTargetSpeed != 0)
  {
    if (MotorData[MotorNr].MotorTargetSpeed > 0)
    {
      MotorData[MotorNr].MotorTargetSpeed--;
    }
    else
    {
      MotorData[MotorNr].MotorTargetSpeed++;
    }
  }

  if (MotorData[MotorNr].MotorSetSpeed != 0)
  {
    if (MotorData[MotorNr].MotorSetSpeed > 0)
    {
      MotorData[MotorNr].MotorSetSpeed--;
    }
    else
    {
      MotorData[MotorNr].MotorSetSpeed++;
    }
  }
}

/* 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)
{
  UBYTE SyncMotorOne;
  UBYTE SyncMotorTwo;

  if (MotorData[MotorNr].RegulationMode & REGSTATE_REGULATED)
  {
    dOutputCalculateMotorPosition(MotorNr);
  }
  else
  {
    if (MotorData[MotorNr].RegulationMode & REGSTATE_SYNCHRONE)
    {
      dOutputMotorSyncStatus(MotorNr, &SyncMotorOne, &SyncMotorTwo);

      if ((SyncMotorOne != 0xFF) &&(SyncMotorTwo != 0xFF))
      {
        dOutputSyncMotorPosition(SyncMotorOne, SyncMotorTwo);
      }
    }
  }
}

/* Regulation function used when Position regulation is enabled */
/* The regulation form only control one motor at a time */
void dOutputCalculateMotorPosition(UBYTE MotorNr)
{
  SWORD PositionError;
  SWORD PValue;
  SWORD IValue;
  SWORD DValue;
  SWORD TotalRegValue;
  SWORD NewSpeedCount = 0;

  NewSpeedCount = (SWORD)((MotorData[MotorNr].MotorTargetSpeed * MAX_CAPTURE_COUNT)/INPUT_SCALE_FACTOR);

  PositionError = (SWORD)(MotorData[MotorNr].OldPositionError - MotorData[MotorNr].DeltaCaptureCount) + NewSpeedCount;

  //Overflow control on PositionError
  if (MotorData[MotorNr].RegPParameter != 0)
  {
    if (PositionError > (SWORD)(32000 / MotorData[MotorNr].RegPParameter))
    {
      PositionError = (SWORD)(32000 / MotorData[MotorNr].RegPParameter);
    }
    if (PositionError < (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter)))
    {
      PositionError = (SWORD)(-(32000 / MotorData[MotorNr].RegPParameter));
    }
  }
  else
  {
    if (PositionError > (SWORD)32000)
    {
      PositionError = (SWORD)32000;
    }
    if (PositionError < (SWORD)-32000)
    {
      PositionError = (SWORD)-32000;
    }
  }
1234

⌨️ 快捷键说明

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