/*============================================================
 * パワーパック(基本機能PWM型) / PIC16F18857
 * by Hirotsugu Kakugawa 
 *============================================================*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "adc.h"
#include "io.h"
#include "pp-const.h"

/*------------------------------------------------------- 
 * MAIN
 *-------------------------------------------------------*/
void main(void) 
{
  /* システム初期化 */
  System_Init();
  /* ウオッチドッグタイマ: 256 [mSec] */
  WDTCON0bits.WDTPS = 0x08; /* 0b01000 */
  CLRWDT();
  /* モータ出力初期化 */
  MOTOR_DIR_NONE();
  MOTOR_PWM_START();
  /* 状態インジケータLED */
  LED_INDICATOR_OFF();
  /* 停車中表示 LED */
  LED_STOPPED_ON();
  /* 主ループ */
  for (;;) {
    __delay_ms(RUN_INTERVAL);
    CLRWDT();
    /* 走行方向を出力 */
    if (SW_DIR1F()) {
      MOTOR_DIR_FWD();
      LED_INDICATOR_ON();
    } else if (SW_DIR1R()) {
      MOTOR_DIR_REV();
      LED_INDICATOR_ON();
    } else {
      MOTOR_DIR_NONE();
      LED_INDICATOR_OFF();
    }
    /* スピードボリューム読込 */
    adc_val_t vr_speed = ADC_Read(ADC_CH_SPEED);
    /* 目標走行速度 */
    /* (可変抵抗器のADC値がちょうど0にならない場合への対策) */
    uint16_t speed = 0;
    if (vr_speed < ADC_MARGIN) {
      speed = 0;
    } else {
      speed = vr_speed - ADC_MARGIN;
    }
    /* モーターPWMデューティー比を出力 */
    uint16_t md = speed; 
    MOTOR_PWM_OUT(md);
    /* 停車中表示 LED */
    if (speed == 0) {
      LED_STOPPED_ON();
    } else {
      LED_STOPPED_OFF();
    }
    /* モータードライバ LSI TB6643KQ 制御 (過電流検出の解除) */
    if (SW_EMG_STOP()) { 
      MotorDriver_TB6643KQ_Reset();
    }
  }
}
