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

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

moto::moto()
{
}
bool moto::init()
{
  Serial.println("init moto");
  pid_param_init(&moto_pid, 16000, 1000, 10, 8000, 0, 3.0f, 0.2f, 0.0f);
  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()
{
}
void moto::update()
{
  pid_cal(&moto_pid, moto_chassis.speed_rpm);
  set_moto_current(moto_pid.output);
}

void moto::settarget(float new_target)
{
  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::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_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)
{

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

  pid->last_err = pid->err;
  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; // 进入死区维持原值
}