#include "by_motion.h"

#include "dwt_delay.h"
#include "by_utils.h"

#define DRV_ENABLE()  gpio_bits_set(GPIOB, GPIO_PINS_15)
#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)

int16_t speed_m1;
int16_t speed_m2;

void by_motion_pwm_m1(int32_t pwm_duty)
{
  pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
  pwm_duty += 499;

  // 互补 pwm 输出，499 为中值
  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
}

void by_motion_pwm_m2(int32_t pwm_duty)
{
  pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
  pwm_duty += 499;

  // 互补 pwm 输出，499 为中值
  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
}

int16_t by_motion_get_speed_m1(void)
{
  speed_m1 = (int16_t)tmr_counter_value_get(TMR2);
  tmr_counter_value_set(TMR2, 0);
  return speed_m1;
}

int16_t by_motion_get_speed_m2(void)
{
  speed_m2 = (int16_t)tmr_counter_value_get(TMR3);
  tmr_counter_value_set(TMR3, 0);
  return speed_m2;
}

void by_motion_init(void)
{
  DRV_ENABLE();
  by_motion_pwm_m1(-125);
  by_motion_pwm_m2(0);
}