/*------------------------------------------------------- 
 * 手動運転モード
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"
#include "pp-sb.h"
#include "ar.h"

/* 最高速度として用いる可変抵抗 */ 
enum max_src_e {
  MAX_SRC_VMAX = 0,
  MAX_SRC_VSLOW,
};

/* 手動運転(自動加減速なし) */
void manual_run_start(void)
{
  /* 最高速度として用いる可変抵抗を選択 */ 
  enum max_src_e max_src = MAX_SRC_VMAX;
  if (!SW_END()) { /* 切替時に END SW 解放: VMAX を使用 */
    max_src = MAX_SRC_VMAX;  
  } else {         /* 切替時に END SW 押下: VSLOW を使用 */
    max_src = MAX_SRC_VSLOW; 
  }
  /* スピードボリュームの定位置復帰を待機 */
  pp_home_position(); 
  /* LED */
  LED_RUN_ON();
  if (max_src == MAX_SRC_VMAX) {
    LED_IDLE_OFF();
  } else {
    LED_IDLE_ON();
  }
  LED_SENS_OFF();
  /* ポイント切替方向 */
  enum if_turnout_dir_e d_curr = TURNOUT_DIR_NONE;
  enum if_turnout_dir_e d_next = TURNOUT_DIR_NONE;
  /* 停止 */
  MOTOR_DIR_NONE();
  pp_sb_duty_set(0);
  /* 主ループ */
  for (;;) {
    pp_delay_ms(RUN_INTERVAL);
    CLRWDT();
    /* モータードライバ LSI TB6643KQ 制御 (過電流検出の解除) */
    if (SW_EMG_STOP()) { /* 解除要求の発生 */
      extern void MotorDriver_TB6643KQ_Reset(void);
      MotorDriver_TB6643KQ_Reset();
    }
    /* 非常停止 */
    if (SW_EMG_STOP()) {
      break;
    }
    /* 現在モード終了 */
    if (SW_MODE_AUTO()) {
      break;
    }
    /* センサ検出 LED 表示 */
    if_sensor_read();
    if (if_sensor_detected()) {
      LED_SENS_ON();
    } else {
      LED_SENS_OFF();
    }
    /* 走行方向を出力(フィーダ1) */
    switch (pp_dir_read()) {
    case DIR_FWD: /* 順方向 */
      ar_motor_on(); /* モータ＆常点灯On  */
      ar_track1_dir_set(AR_TRACK_DIR_NORMAL);
      break;
    case DIR_REV: /* 逆方向 */
      ar_motor_on(); /* モータ＆常点灯On  */
      ar_track1_dir_set(AR_TRACK_DIR_REVERSE);
      break;
    default:
      ar_motor_off(); /* モータ＆常点灯Off  */
      break;
    }
    /* 走行方向を出力(フィーダ2) */
    if (SW_DIR2F()) { /* 順方向 */
      ar_track2_dir_set(AR_TRACK_DIR_NORMAL);
    } else {          /* 逆方向 */
      ar_track2_dir_set(AR_TRACK_DIR_REVERSE);
    }
    /* ポイント切替 */
    if (!if_toq_next(RUN_INTERVAL)) {
      if (SW_TURNOUT()) {
        d_next = TURNOUT_DIR_NORMAL;  /* 定位 */
      } else {
        d_next = TURNOUT_DIR_REVERSE; /* 反位 */
      }
      if (d_curr != d_next) { /* 切替 */
        d_curr = d_next;
        if (d_curr == TURNOUT_DIR_NORMAL) {
          if_toq_set_all_normal();
        } else {
          if_toq_set_all_reverse();
        }
      }
    }
    /* パラメータの読込 */
    ar_param_read();
    /* 現在走行速度 */
    uint16_t speed = p_speed;
    /* 走行速度を変換して, モーターPWMデューティー比を得る */
    uint16_t md = 0;
    if (max_src == MAX_SRC_VMAX) {
      md = PP_CONV_SPEED_TO_MDUTY(p_speed, p_vmax);
    } else {
      md = PP_CONV_SPEED_TO_MDUTY(p_speed, p_vslow);
    }
    /* モーターPWMデューティー比を出力 */
    pp_sb_duty_set(md);
  }
}
