#include "moto.h"
#include "Arduino.h"

uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis;
PID_TypeDef moto_pid;

moto::moto()
{
  _runing = false;
  brake = true;
  _closed=false;
}
bool moto::init()
{
  Serial.println("init moto");
  pid_init();
  CAN.setPins(26, 27);
  // start the CAN bus at 500 kbps
  if (!CAN.begin(1000E3))
  {
    Serial.println("Starting CAN failed!");
    return false;
  }
  CAN.onReceive(onReceive);
  return true;
}
moto::~moto()
{
}

// int16_t delta;
// int16_t max_speed_change = 100; // 500 ok  100电源不报高压错误但有点震荡
// int16_t set_speed_temp;
void moto::update()
{

  //  //可以柔和改变速度，柔和程度max_speed_change越小越柔和,但太小需要调PID
  // for (uint8_t i = 0; i < 2; i++)
  // {
  //   // 加减速
  //   delta = (int16_t)moto_pid.target - moto_chassis.speed_rpm;
  //   if (delta > max_speed_change)
  //     set_speed_temp = (float)(moto_chassis.speed_rpm + max_speed_change);
  //   else if (delta < -max_speed_change)
  //     set_speed_temp = (float)(moto_chassis.speed_rpm - max_speed_change);
  //   else
  //     set_speed_temp = moto_pid.target;
  //   pid_cal(&moto_pid, moto_chassis.speed_rpm,set_speed_temp);
  // }

  if ((_runing || brake)&&!_closed)
  {
    pid_cal(&moto_pid, moto_chassis.speed_rpm, moto_pid.target);
    set_moto_current(moto_pid.output);
  }
}

// void moto::update()
// {
//   pid_cal(&moto_pid, moto_chassis.speed_rpm);
//   set_moto_current(moto_pid.output);
// }

void moto::settarget(float new_target)
{
  // 为了马上停止，跳过PID控制器
  if (new_target == 0.0f)
  { 
    // 没刹车直接断开电流控制
    _runing = false;
    if (!brake)
    {
      pid_init();
      set_moto_current(0);
    }
  }
  else
  {  _runing = true;
     _closed=false;
  }

  moto_pid.target = new_target * MOTO_REDUCTION;
}

void moto::set_moto_current(int16_t iq1)
{
  iq1 = MOTO_REVERSED * iq1;
  CAN.beginPacket(0x200, 8);
  CAN.write((uint8_t)((iq1 >> 8) & 0xFF));
  CAN.write((uint8_t)(iq1 & 0xFF));
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.write(0x00);
  CAN.endPacket();
}
void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr)
{
  // 处理数据
  ptr->last_angle = ptr->angle;
  ptr->angle = (uint16_t)MOTO_REVERSED * (message_ptr[0] << 8 | message_ptr[1]);       // 角度
  ptr->speed_rpm = (int16_t)MOTO_REVERSED * (message_ptr[2] << 8 | message_ptr[3]);    // 速度
  ptr->real_current = (int16_t)MOTO_REVERSED * (message_ptr[4] << 8 | message_ptr[5]); // 实际转矩电流
  ptr->temperature = message_ptr[6];                                                   // 电机温度，2006没有，为0

  if (ptr->angle - ptr->last_angle > MOTO_MAXANG_HALF)
    ptr->round_cnt--;
  else if (ptr->angle - ptr->last_angle < -MOTO_MAXANG_HALF)
    ptr->round_cnt++;
  ptr->total_angle = ptr->round_cnt * MOTO_MAXANG + ptr->angle - ptr->offset_angle;

  // ptr->output_speed_rpm= (float)ptr->speed_rpm / MOTO_REDUCTION;
  //  ptr->output_round_cnt=  ptr->round_cnt  + (float)(ptr->angle - ptr->offset_angle)/MOTO_MAXANG/ MOTO_REDUCTION;
}

moto_measure_t moto::getmotoinfo()
{
  // 在中断里面计算时间太长会导致重启,在取信息时计算浮点
  moto_chassis.output_speed_rpm = (float)moto_chassis.speed_rpm / MOTO_REDUCTION;
  moto_chassis.output_round_cnt = (moto_chassis.round_cnt + (float)(moto_chassis.angle - moto_chassis.offset_angle) / MOTO_MAXANG) / MOTO_REDUCTION;
  return moto_chassis;
}
//关闭电机输出，防止堵转
void moto::close()
{
   set_moto_current(0);
   _closed=true;
}
void moto::onReceive(int packetSize)
{
  if (!CAN.packetRtr())
  {
    for (int i = 0; i < packetSize; i++)
    {
      CaninBuffer[i] = CAN.read();
    }
    get_moto_measure(&moto_chassis, CaninBuffer);
  }
}
/////////////PID控制
void pid_init()
{
  pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 10.0f, 1.0f, 0.5f); //
  // pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 14.0f, 0.01f, 0.0f); ////
}
void pid_param_init(PID_TypeDef *pid,
                    uint16_t maxout,
                    uint16_t intergral_limit,
                    float deadband,
                    int16_t max_err,
                    int16_t target,
                    float kp,
                    float ki,
                    float kd)
{

  memset(pid, 0, sizeof(PID_TypeDef));

  pid->DeadBand = deadband;
  pid->IntegralLimit = intergral_limit;
  pid->MaxOutput = maxout;
  pid->Max_Err = max_err;
  pid->target = target;

  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;

  pid->output = 0;
}

inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd)
{
  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;
}

inline void pid_target(PID_TypeDef *pid, float new_target)
{
  pid->target = new_target;
}

float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget)
{

  pid->lasttime = pid->thistime;
  pid->thistime = xTaskGetTickCount() * portTICK_RATE_MS; // 换算为绝对时间
  pid->dtime = pid->thistime - pid->lasttime;             // 更新两次计算的时间

  pid->last_err = pid->err;
  pid->err = pidtarget - measure;
  // pid->err = pid->target - measure;

  if (abs(pid->err) > pid->DeadBand)
  {
    pid->pout = pid->kp * pid->err;
    pid->iout += (pid->ki * pid->err);
    pid->dout = pid->kd * (pid->err - pid->last_err); // 除以dtime？？

    // 积分限幅
    if (pid->iout > pid->IntegralLimit)
      pid->iout = pid->IntegralLimit;
    if (pid->iout < -pid->IntegralLimit)
      pid->iout = -pid->IntegralLimit;

    pid->output = pid->pout + pid->iout + pid->dout;

    if (pid->output > pid->MaxOutput)
    {
      pid->output = pid->MaxOutput;
    }
    if (pid->output < -(pid->MaxOutput))
    {
      pid->output = -(pid->MaxOutput);
    }
  }

  return pid->output; // 进入死区维持原值
}