d_output.c

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

      {
        //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))
  {
    if (MotorData[MotorOne].TurnParameter != 0)
    {
      if (MotorData[MotorOne].TurnParameter > 0)
      {
        if (MotorData[MotorTwo].MotorTargetSpeed >= 0)
        {
          if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun))
          {
            MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
            MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;

            MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount;
            MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun;
          }
        }
        else
        {
          if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun))
          {
            MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
            MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;

            MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount;
            MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun;
          }
        }
      }
      else
      {
        if (MotorData[MotorOne].MotorTargetSpeed >= 0)
        {
          if ((SLONG)(MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun))
          {
            MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
            MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;

            MotorData[MotorTwo].CurrentCaptureCount = MotorData[MotorOne].CurrentCaptureCount;
            MotorData[MotorTwo].MotorTachoCountToRun = MotorData[MotorOne].MotorTachoCountToRun;
          }
        }
        else
        {
          if ((SLONG)(MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun))
          {
            MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
            MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;

            MotorData[MotorOne].CurrentCaptureCount = MotorData[MotorTwo].CurrentCaptureCount;
            MotorData[MotorOne].MotorTachoCountToRun = MotorData[MotorTwo].MotorTachoCountToRun;
          }
        }
      }
    }
    else
    {
      if (MotorData[MotorOne].MotorSetSpeed > 0)
      {
        if ((MotorData[MotorOne].CurrentCaptureCount >= MotorData[MotorOne].MotorTachoCountToRun) || (MotorData[MotorTwo].CurrentCaptureCount >= MotorData[MotorTwo].MotorTachoCountToRun))
        {
          MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
          MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
        }
      }
      else
      {
        if (MotorData[MotorOne].MotorSetSpeed < 0)
        {
          if ((MotorData[MotorOne].CurrentCaptureCount <= MotorData[MotorOne].MotorTachoCountToRun) || (MotorData[MotorTwo].CurrentCaptureCount <= MotorData[MotorTwo].MotorTachoCountToRun))
          {
            MotorData[MotorOne].MotorRunState = MOTOR_RUN_STATE_IDLE;
            MotorData[MotorTwo].MotorRunState = MOTOR_RUN_STATE_IDLE;
          }
        }
      }
    }
  }
}

/* Function which can evaluate which motor are synched */
void dOutputMotorSyncStatus(UBYTE MotorNr, UBYTE *SyncMotorOne, UBYTE *SyncMotorTwo)
{
  if (MotorNr < MOTOR_C)
  {
    if (MotorNr == MOTOR_A)
    {
      *SyncMotorOne = MotorNr;
      *SyncMotorTwo = *SyncMotorOne + 1;
      if (MotorData[*SyncMotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
      {
        //Synchronise motor A & B
      }
      else
      {
        *SyncMotorTwo = *SyncMotorOne + 2;
        if (MotorData[*SyncMotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
        {
          //Synchronise motor A & C
        }
        else
        {
          //Only Motor A has Sync setting => Do nothing, treat motor as motor without regulation
          *SyncMotorTwo = 0xFF;
        }
      }
    }
    if (MotorNr == MOTOR_B)
    {
      *SyncMotorOne = MotorNr;
      *SyncMotorTwo = *SyncMotorOne + 1;
      if (MotorData[*SyncMotorTwo].RegulationMode & REGSTATE_SYNCHRONE)
      {
        if (!(MotorData[MOTOR_A].RegulationMode & REGSTATE_SYNCHRONE))
        {
          //Synchronise motor B & C
        }
      }
      else
      {
        //Only Motor B has Sync settings or Motor is sync. with Motor A and has therefore already been called
        *SyncMotorTwo = 0xFF;
      }
    }
  }
  else
  {
    *SyncMotorOne = 0xFF;
    *SyncMotorTwo = 0xFF;
  }
}
/* Function which is called when motors are synchronized and the motor position is reset */
void dOutputResetSyncMotors(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
      {
        //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;
      }
    }
  }

  if ((MotorOne != 0xFF) && (MotorTwo != 0xFF))
  {
    MotorData[MotorOne].CurrentCaptureCount = 0;
    MotorData[MotorOne].MotorTachoCountToRun = 0;
    MotorData[MotorTwo].CurrentCaptureCount = 0;
    MotorData[MotorTwo].MotorTachoCountToRun = 0;
  }
  else
  {
    MotorData[MotorNr].CurrentCaptureCount = 0;
    MotorData[MotorNr].MotorTachoCountToRun = 0;
  }
}

/* Function which is called when motors are synchronized and motor is ramping down */
void dOutputRampDownSynch(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
      {
        //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))
  {
    if (MotorData[MotorOne].TurnParameter != 0)
    {
      if (MotorData[MotorOne].TurnParameter > 0)
      {
        if (MotorData[MotorOne].MotorTargetSpeed >= 0)
        {
          if (MotorData[MotorTwo].MotorActualSpeed < 0)
          {
            MotorData[MotorTwo].MotorTargetSpeed--;
          }
        }
        else
        {
          if (MotorData[MotorTwo].MotorActualSpeed > 0)
          {
            MotorData[MotorTwo].MotorTargetSpeed++;
          }
        }
      }
      else
      {
        if (MotorData[MotorOne].MotorTargetSpeed >= 0)
        {
          if (MotorData[MotorTwo].MotorActualSpeed < 0)
          {
            MotorData[MotorTwo].MotorTargetSpeed--;
          }
        }
        else
        {
          if (MotorData[MotorTwo].MotorActualSpeed > 0)
          {
            MotorData[MotorTwo].MotorTargetSpeed++;
          }
        }
      }
    }
  }
}