/***********************************************
 * サーボモーターコントローラ / PIC16F18326
 * by Hirotsugu Kakugawa
 ************************************************/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "adc.h"
#include "io.h"
#include "servo.h"
#include "turnout.h"
#include "incr-lin.h"

/* ----------------------------------------------------- */

/* 移動速度の係数（大きいほど早くなる） */
#define SPEED_NORMAL   1024
#define SPEED_SLOW      128

/* 移動速度設定値の下限（この値以下だと即座に移動） */
#define SPEED_VR_THR     4

/* 設定SW */
enum e_degree_range sw_degree_range = DEGREE_RANGE_90;
enum e_move_speed sw_move_speed     = MOVE_SPEED_NORMAL;

/* サーボモータ移動速度 */
uint16_t speed = 0;
/* 回転角度: 位置A (定位) */
uint16_t pos_target_a = 0;
/* 回転角度: 位置B (反位) */
uint16_t pos_target_b = 0;
/* 速度定数 */
uint16_t spd_const = 0;

/* 有限状態機械 */
enum e_state {
  STATE_A,  /* 位置A(定位) */
  STATE_B,  /* 位置B(反位) */
};

static void param_read(void);

/*------------------------------------------------------- 
 * MAIN
 *-------------------------------------------------------*/
void main(void)
{
  /* システム初期化 */
  System_Init();
  /* ウオッチドッグタイマ: 256 [mSec] */
  WDTCONbits.WDTPS  = 0x08; /* 0b01000 */
  CLRWDT();
  /* 開通方向出力: 切替動作中 */
  OUT_OD_DIR_A_LOW(); /* 位置A(定位): 非開通 */
  OUT_OD_DIR_B_LOW(); /* 位置B(反位): 非開通 */
  /* 入力・パラメータ読込 */
  param_read();
  /* サーボモータ現在回転角度 */
  uint16_t pos_current = 0;
  /* 初期ポイント制御入力を読む */
  enum e_movereq movereq = instate_init();
  /* 制御状態とパラメータの初期化 */
  enum e_state state = STATE_A;
  switch (movereq) {
  case MOVEREQ_NONE:
  case MOVEREQ_A:
    state = STATE_A;
    pos_current = pos_target_a;
    break;
  case MOVEREQ_B:
    state = STATE_B;
    pos_current = pos_target_b;
  }
  incr_lin_init(pos_current);
  /* 主ループ */
  for (;;) {
    CLRWDT();
    /* 入力・パラメータ読込 */
    param_read();
    /* ポイント制御入力を読む */
    movereq = instate_next();
    /* サーボモータ制御 */
    switch (state) {
    case STATE_A: /* 位置A(定位) */
      if (pos_current == pos_target_a) { /* 切替完了 */
        /* 開通方向出力: 位置A(定位) */
        OUT_OD_DIR_A_Z();   /* 位置A(定位): 開通 */
        OUT_OD_DIR_B_LOW(); /* 位置B(反位): 非開通 */
      } else {      /* 切替中 */ 
        /* 現在位置を更新 */
        if (speed == 0) { /* 即座 */
          pos_current = pos_target_a;
        } else {                   /* ゆっくり */
          pos_current
            = incr_lin_next(speed, spd_const, pos_target_a);
        }
      }
      /* 切替要求 */
      switch (movereq) {
      case MOVEREQ_NONE:
        break;
      case MOVEREQ_A: /* 位置A(定位)への切替要求 */ 
        break;
      case MOVEREQ_B: /* 位置B(反位)への切替要求 */ 
        /* 切替動作中 */
        state = STATE_B;
        incr_lin_init(pos_current);
        OUT_OD_DIR_A_LOW(); /* 位置A(定位): 非開通 */
        OUT_OD_DIR_B_LOW(); /* 位置B(反位): 非開通 */
        break;
      }
      break;
    case STATE_B: /* 位置B(反位) */
      if (pos_current == pos_target_b) { /* 切替完了 */
        /* 開通方向出力: 位置B(反位) */
        OUT_OD_DIR_A_LOW(); /* 位置A(定位): 非開通 */
        OUT_OD_DIR_B_Z();   /* 位置B(反位): 開通 */
      } else {      /* 切替中 */
        /* 現在位置を更新 */
        if (speed == 0) { /* 即座 */
          pos_current = pos_target_b;
        } else {                   /* ゆっくり */
          pos_current
            = incr_lin_next(speed, spd_const, pos_target_b);
        }
      }
      /* 切替要求 */
      switch (movereq) {
      case MOVEREQ_NONE:
        break;
      case MOVEREQ_A: /* 位置A(定位)への切替要求 */ 
        /* 切替動作中 */
        state = STATE_A;
        incr_lin_init(pos_current);
        OUT_OD_DIR_A_LOW(); /* 位置A(定位): 非開通 */
        OUT_OD_DIR_B_LOW(); /* 位置B(反位): 非開通 */
        break;
      case MOVEREQ_B: /* 位置B(反位)への切替要求 */ 
        break;
      }
      break;
    }
    /* サーボモータへパルスを送信 */
    servo_pulse(pos_current);
  }
}

static void param_read(void)
{
  /* 可変抵抗器読取 */
  adc_val_t adc_speed = ADC_Read(ADC_CH_SPEED);
  if (adc_speed <= SPEED_VR_THR) {
    speed = 0;
  } else {
    speed = adc_speed >> 2;
  }
  pos_target_a = ADC_Read(ADC_CH_POS_A);
  pos_target_b = ADC_Read(ADC_CH_POS_B);
  /* 設定スイッチ読取: 可動角度範囲 */
  sw_degree_range = DEGREE_RANGE_90;
  if (!SW_DEGREE_RANGE_90()) {
    sw_degree_range = DEGREE_RANGE_180;
  }
  /* 設定スイッチ読取: 移動速度 */ 
  sw_move_speed = MOVE_SPEED_NORMAL;
  if (!SW_MOVE_SPEED_NORMAL()) {
    sw_move_speed = MOVE_SPEED_SLOW;
  }
  switch (sw_move_speed) {
  default:
  case MOVE_SPEED_NORMAL:
    spd_const = SPEED_NORMAL;
    break;
  case MOVE_SPEED_SLOW:
    spd_const = SPEED_SLOW;
    break;
  }
}
