/*-------------------------------------------------------
 * パワーパック関係  諸関数
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "adc.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"

/*-------------------------------------------------------
 * 速度ボリュームの定位置復帰を待機 
 *-------------------------------------------------------*/
void pp_home_position(void)
{
  for (;;) {
    CLRWDT();
    /* ボリューム読み込み */ 
    pp_param_read();
    if (p_speed == 0) { /* 定位置へ復帰 */
      break;
    }
    /* 未復帰: インジケータLEDを点滅 */
    LED_INDICATOR_TOGGLE();
    pp_delay_ms(T_LED_WARNING);
    /* モータードライバ LSI TB6643KQ 制御 (過電流検出の解除) */
    if (SW_EMG_STOP()) { /* 解除要求の発生 */
      extern void MotorDriver_TB6643KQ_Reset(void);
      MotorDriver_TB6643KQ_Reset();
    }
  }
  LED_INDICATOR_OFF();
}

/*-------------------------------------------------------
 * 目標走行速度を求める
 *-------------------------------------------------------*/
uint16_t pp_controler_speed(uint16_t speed)
{
  return p_speed;
}

/*-------------------------------------------------------
 * 加速度&減速度を求める
 *-------------------------------------------------------*/
uint16_t pp_controler_accel(uint16_t target_speed,
                            uint16_t speed)
{
  uint16_t a = 0;
  if (target_speed > speed) { 
    a = p_accel; /* 加速 */
  } else { 
    a = p_decel; /* 減速 */
  }
  return a;
}
