96 lines
2.7 KiB
C++
96 lines
2.7 KiB
C++
#pragma once
|
||
#include <Arduino.h>
|
||
#include <CAN.h>
|
||
#include <ESP32SJA1000.h>
|
||
#define MOTO_REVERSED -1 // 正反转1为正转，-1为反转
|
||
#define MOTO_REDUCTION 36
|
||
#define MOTO_MAXANG 8192 // 0-8191
|
||
#define MOTO_MAXANG_HALF MOTO_MAXANG / 2
|
||
|
||
#define OUTPUT_MAX 400 // 最大输出转速rpm
|
||
#define SACCEL_TIME 100 // 使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就 (200*SACCEL_TIME)/OUTPUT_MAX 秒
|
||
|
||
typedef struct _PID_TypeDef
|
||
{
|
||
float target; // 目标值====
|
||
float lastNoneZeroTarget;
|
||
float kp;
|
||
float ki;
|
||
float kd;
|
||
float measure; // 测量值
|
||
float err; // 误差
|
||
float last_err; // 上次误差
|
||
float pout;
|
||
float iout;
|
||
float dout;
|
||
float output; // 本次输出====
|
||
// float last_output; //上次输出
|
||
float MaxOutput; // 输出限幅=====
|
||
float IntegralLimit; // 积分限幅=====
|
||
float DeadBand; // 死区（绝对值）=====
|
||
// float ControlPeriod; //控制周期====
|
||
float Max_Err; // 最大误差====
|
||
|
||
uint32_t thistime;
|
||
uint32_t lasttime;
|
||
uint8_t dtime;
|
||
|
||
} PID_TypeDef;
|
||
|
||
typedef struct
|
||
{
|
||
int16_t speed_rpm;
|
||
int16_t real_current;
|
||
uint8_t temperature;
|
||
uint16_t angle; // abs angle range:[0,8191]
|
||
uint16_t last_angle; // abs angle range:[0,8191]
|
||
uint16_t offset_angle;
|
||
int32_t round_cnt;
|
||
int32_t total_angle;
|
||
uint32_t msg_cnt;
|
||
|
||
unsigned long starttime;
|
||
|
||
float output_speed_rpm; // 输出轴速度 ---rpm
|
||
float output_round_cnt; // 输出轴 总圈数
|
||
} moto_measure_t;
|
||
|
||
/// 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);
|
||
inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd);
|
||
inline void pid_target(PID_TypeDef *pid, float new_target);
|
||
float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget);
|
||
void pid_init();
|
||
|
||
class moto
|
||
{
|
||
private:
|
||
bool _closed;
|
||
float _ds;
|
||
int16_t _start_speed;
|
||
float _acctime;
|
||
void set_moto_current(int16_t iq1);
|
||
static void get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr);
|
||
static void onReceive(int packetSize);
|
||
|
||
public:
|
||
moto(); // 构造函数
|
||
~moto(); // 析构函数
|
||
bool init();
|
||
void update();
|
||
// 设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0)
|
||
// 否则setspeedtarget(0)会按照settime_acc的时间停止
|
||
void setspeedtarget(float new_target);
|
||
void settime_acc(float acctime = -1); // 设置加速和减速时间
|
||
void close(); // 关闭电机供电
|
||
moto_measure_t getmotoinfo();
|
||
};
|