srvsld.c

本程序为ST公司开发的源代码

    sledge_kick();
    start_timer(SERVO_SLEDGE_TIMER, SLEDGE_PRO_KICK_TIME);
    sledge_fsm_state = SLEDGE_STATE_MOVING_KICK;
    break;

  case SLEDGE_STATE_MOVING_KICK:
    if (!timer_in_progress(SERVO_SLEDGE_TIMER))
    {
      sledge_constant();
      start_timer(SERVO_SLEDGE_TIMER, absi(sledge_param));
      sledge_fsm_state = SLEDGE_STATE_MOVING_CONSTANT;
    }  
    break;

  case SLEDGE_STATE_MOVING_CONSTANT:
    if (!timer_in_progress(SERVO_SLEDGE_TIMER))
    {
      sledge_brake();
      start_timer(SERVO_SLEDGE_TIMER, SLEDGE_PRO_BRAKE_TIME);
      sledge_fsm_state = SLEDGE_STATE_MOVING_BRAKE;
    }
    break;

  case SLEDGE_STATE_MOVING_CHANGE_PARAM:
    start_timer(SERVO_SLEDGE_TIMER, absi(sledge_param));
    sledge_fsm_state = SLEDGE_STATE_MOVING_CONSTANT;
    break;

  //case SLEDGE_STATE_BRAKING:
  //case SLEDGE_STATE_MOVING_BRAKE:
  default:
    if (!timer_in_progress(SERVO_SLEDGE_TIMER))
    {
      sledge_idle();
      sledge_fsm_state = SLEDGE_STATE_IDLE;
      sledge_status_event = SLEDGE_STATUS_EVENT_STOPPED;
      DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_EVENT: Sledge stopped", 4, 0));
    }
    break;
  }
  
  return sledge_status_event;
}


/******************************************************************************/
/* Function:  sledge_fsm_call                                                 */
/*                                                                            */
/*! \brief    external call to sledge_fsm
 *  \param    t_sledge_caller_sledge_caller
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void sledge_fsm_call(t_sledge_caller sledge_caller)
{
  t_sledge_sw_event sledge_sw_event;
  t_sledge_status_event sledge_status_event;

  event_disable_scheduling();
  sledge_sw_event = sledge_update(sledge_caller);
  set_sledge_sw_event(sledge_sw_event);
  event_out_shedule(SLEDGE_SW_EVENT);
  sledge_status_event = sledge_fsm(sledge_caller);
  if (sledge_caller < SLEDGE_CALLER_MAX_CMD)
  {
    if (SLEDGE_STATUS_NO_EVENT != sledge_status_event)
    {
      event_pending[SLEDGE_STATUS_EVENT].event = SLEDGE_STATUS_NO_EVENT;
      event_in[SLEDGE_STATUS_EVENT].event = sledge_status_event;
    }
  }
  else
  {
    set_sledge_status_event(sledge_status_event);
    event_out_shedule(SLEDGE_STATUS_EVENT);
  }
  if (servo_sledge_flags.field.ttm_fsm_call_request)
  {
    servo_sledge_flags.field.ttm_fsm_call_request = 0;
    ttm_fsm_call(TTM_CALLER_DELTA_TRK_OK);
  }
  event_enable_scheduling();
}


/******************************************************************************/
/* Function:  sledge_stopped                                                  */
/*                                                                            */
/*! \brief    Check if sledge fsm is active
 *  \param    void
 *  \return   t_bool TRUE/FALSE
 *  \remark
 */
/******************************************************************************/

t_bool sledge_stopped(void)
{
  return (t_bool)(SLEDGE_STATUS_EVENT_STOPPED == event_in[SLEDGE_STATUS_EVENT].event);
}


/******************************************************************************/
/* Function:  dc_start_sledge                                                 */
/*                                                                            */
/*! \brief    Activates sledge_fsm
 *  \param    uint16 speed, uint16 acceleration, int16 number of steps
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void dc_start_sledge (void)
{
  DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_CMD_START", 4, 0));
  sledge_fsm_call(SLEDGE_CALLER_START_CMD);
}


/******************************************************************************/
/* Function:  dc_stop_sledge                                                  */
/*                                                                            */
/*! \brief    Break a jump with the provided braking acceleration
 *  \param    uint16 acceleration
 *  \return   void
 *  \remark
 */
/******************************************************************************/

void dc_stop_sledge (void)
{
  DEBUG_SERVO_SLEDGE_EVENT(("SLEDGE_CMD_STOP", 4, 0));
  sledge_fsm_call(SLEDGE_CALLER_STOP_CMD);
}


/******************************************************************************/
/* Function:  sledge_home                                                     */
/*                                                                            */
/*! \brief    Moving sledge into home position (state machine)
 *  \param    void
 *  \return   uint8 OK, IN_PROGRESS, ERROR
 *  \remark
 */
/******************************************************************************/

RETVAL sledge_home(void)
{
  RETVAL sledge_home_result;

  sledge_home_result = IN_PROGRESS;

  switch (sledge_home_state)
  {
  case STATE_1:
    DEBUG_SERVO (("sledge_home", 4, 0));
    sledge_condition = SLEDGE_CONDITION_HOME_POSITIONING;
    if (!home_position())
    {
      sledge_param = SLEDGE_HOME_PRO_INSIDE_PARAM;
      dc_start_sledge();
      sledge_home_state = STATE_2;
    }
    else   /* (home_position()) */
    {
      DEBUG_SERVO (("home_pos reached state_1", 4, 0));
      sledge_param = SLEDGE_HOME_PRO_OUTSIDE_PARAM;
      dc_start_sledge();
      sledge_home_state = STATE_4;
    }
    break;

  case STATE_2:
    if (home_position())
    {
      DEBUG_SERVO (("home_pos reached state_2", 4, 0));
      dc_stop_sledge();
      sledge_home_state = STATE_3;
    }
    else   /* (!home_position()) */
    {
      if (sledge_stopped())
      {
        sledge_home_result = SLEDGE_ERROR;   /* sledge blocked */
      }
      /* else not needed   (sledge moving) */
    }
    break;

  case STATE_3:
    if (sledge_stopped())
    {
      if (home_position())
      {
        sledge_param = SLEDGE_HOME_PRO_OUTSIDE_PARAM;
        dc_start_sledge();
        sledge_home_state = STATE_4;
      }
      else   /* (!home_position()) */
      {
        start_timer(SERVO_FUNCTION_TIMER, SLEDGE_HOME_PRO_EXTRA_TIME);
        sledge_param = SLEDGE_HOME_PRO_OUTSIDE_PARAM;
        dc_start_sledge();
        sledge_home_state = STATE_5;
      }
    }
    /* else not needed   (sledge moving) */
    break;

  case STATE_4:
    if (!home_position())
    {
      start_timer(SERVO_FUNCTION_TIMER, SLEDGE_HOME_PRO_EXTRA_TIME);
      sledge_home_state = STATE_5;
    }
    else   /* (home_position()) */
    {
      if (sledge_stopped())
      {
        sledge_home_result = SLEDGE_ERROR;   /* sledge blocked */
      }
      /* else not needed   (sledge moving) */
    }
    break;

  case STATE_5:
    if (!timer_in_progress(SERVO_FUNCTION_TIMER))
    {
      dc_stop_sledge();
      sledge_home_state = STATE_6;
    }
    /* else not needed   (timer_in_progress(SERVO_FUNCTION_TIMER)) */
    break;
    
  //case STATE_6:
  default:
    if (sledge_stopped())
    {
      sledge_condition = SLEDGE_CONDITION_NORMAL;
      sledge_home_result = OK;
      DEBUG_SERVO (("sledge_home OK", 4, 0));
    }
    /* else not needed   (sledge moving) */
    break;
  }

  return sledge_home_result;
}


/******************************************************************************/
/* Function:  sledge_move                                                     */
/*                                                                            */
/*! \brief    jump without checking for HF or FOCUS (focus revocery)
 *  \param    int16 number of steps
 *  \return   uint8 OK, IN_PROGRESS, ERROR
 *  \remark
 */
/******************************************************************************/

RETVAL sledge_move(void)
{
  RETVAL sledge_move_result;

  sledge_move_result = IN_PROGRESS;

  switch (sledge_move_state)
  {
  case STATE_1:
    DEBUG_SERVO (("sledge_move", 4, 0));
    dc_start_sledge();
    sledge_move_state = STATE_2;
    break;

  //case STATE_2:
  default:
    if (sledge_stopped())
    {
      sledge_move_result = OK;
    }
    /* else not needed   (sledge moving) */
    break;
  }

  return sledge_move_result;
}


/******************************************************************************/
/* Function:  sledge_move_hf                                                  */
/*                                                                            */
/*! \brief    Recovery jump from HF LOST condition
 *  \param    int16 number of steps
 *  \return   uint8 OK, IN_PROGRESS, ERROR
 *  \remark
 */
/******************************************************************************/

RETVAL sledge_move_hf(void)
{
  RETVAL sledge_move_hf_result;

  sledge_move_hf_result = IN_PROGRESS;

  if (check_servo(CHECK_FOCUS) == OK)
  {
    switch (sledge_move_hf_state)
    {
    case STATE_1:
      DEBUG_SERVO (("sledge_move_hf", 4, 0));
      dc_start_sledge();
      start_timer(SERVO_FUNCTION_TIMER, T4ms);
      sledge_move_hf_state = STATE_2;
      break;

    case STATE_2:
      if (!timer_in_progress(SERVO_FUNCTION_TIMER))
      {
        sledge_move_hf_state = STATE_3;
      }
      break;

    case STATE_3:
      if (sledge_stopped())
      {
        sledge_move_hf_result = HF_NOT_FOUND;
      }
      else if (hf_ok())
      {
        sledge_param = sign(sledge_param) * RECOVER_PRO_HF_RECOVER_EXTRA_TIME;
        dc_start_sledge();
        sledge_move_hf_state = STATE_4;
      }
      break;

    //case STATE_4:
    default:
      if (sledge_stopped())
      {
        sledge_move_hf_result = OK;
      }
      break;
    }
  }
  else
  {
    sledge_move_hf_result = FOCUS_LOST;
  }
  /* else not needed ((sledge_move_hf = check_servo(CHECK_FOCUS)) == FOCUS_LOST) */

  return sledge_move_hf_result;
}


/******************************************************************************/
/* Function:  sledge_move_long_jump                                           */
/*                                                                            */
/*! \brief    Execute a long jump (sledge master, tracking servo off)
 *  \param    int16 number of steps
 *  \return   uint8 IN_PROGRESS, OK, ERROR
 *  \remark
 */
/******************************************************************************/

RETVAL sledge_move_long_jump(void)
{
  RETVAL sledge_move_long_jump_result;
  
  sledge_move_long_jump_result = check_servo(CHECK_TTM | CHECK_FOCUS | CHECK_HF);
  if (sledge_move_long_jump_result == OK)
  {
    sledge_move_long_jump_result = IN_PROGRESS;

    switch (sledge_move_long_jump_state)
    {
    case STATE_1:
      DEBUG_SERVO (("sledge_move_long_jump", 4, 0));
      dc_start_sledge();
      sledge_condition = SLEDGE_CONDITION_CHECK_LONG_JUMP_DISTANCE;
      sledge_move_long_jump_state = STATE_2;
      break;

    case STATE_2:
      if (sledge_stopped())
      {
        sledge_move_long_jump_result = OK;
      }
      else if (servo_seek_flags.field.seek_state == NEW_SEEK_REQUESTED)
      {
        sledge_condition = SLEDGE_CONDITION_NORMAL;
        dc_stop_sledge();
        sledge_move_long_jump_state = STATE_3;
      }
      break;

    //case STATE_3:
    default:
      if (sledge_stopped())
      {
        sledge_move_long_jump_result = OK;
      }
      break;
    }
  }
  else   /* ((sledge_move_long_jump_result = check_servo(CHECK_TTM | CHECK_FOCUS | CHECK_HF)) == TTM_STOPPED / FOCUS_LOST / HF_LOST) */
  {
    sledge_condition = SLEDGE_CONDITION_NORMAL;
  }

  return sledge_move_long_jump_result;
}


/******************************************************************************/
/* Function:  sledge_move_relock                                              */
/*                                                                            */
/*! \brief    Move sledge and check for FOCUS and HF availability
 *  \param    int16 number of steps
 *  \return   uint8 OK, IN_PROGRESS
 *  \remark
 */
/******************************************************************************/

RETVAL sledge_move_relock(void)
{
  RETVAL sledge_move_relock_result;

  sledge_move_relock_result = check_servo(CHECK_FOCUS | CHECK_HF);
  if (sledge_move_relock_result == OK)
  {
    sledge_move_relock_result = IN_PROGRESS;

    switch (sledge_move_relock_state)
    {
    case STATE_1:
      DEBUG_SERVO (("sledge_move_relock", 4, 0));
      dc_start_sledge();
      sledge_move_relock_state = STATE_2;
      break;

    //case STATE_2:
    default:
      if (sledge_stopped())
      {
        sledge_move_relock_result = OK;
      }
      break;
    }
  }
  /* else not needed   ((sledge_move_relock_result = check_servo(CHECK_FOCUS | CHECK_HF)) == TTM_STOPPED / FOCUS_LOST / HF_LOST) */

  return sledge_move_relock_result;
}


#endif // HAVE_CD_MECHA

/*** (c) 2003  STMicroelectronics **************************** END OF FILE ***/