224 lines
5.8 KiB
C++
224 lines
5.8 KiB
C++
#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; // 进入死区维持原值
|
||
}
|