diff --git a/.vscode/settings.json b/.vscode/settings.json index 8f0f915..b89a90e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,6 +2,14 @@ "files.associations": { "cstddef": "cpp", "algorithm": "cpp", - "cmath": "cpp" + "cmath": "cpp", + "array": "cpp", + "string": "cpp", + "string_view": "cpp", + "chrono": "cpp", + "functional": "cpp", + "ios": "cpp", + "xlocale": "cpp", + "xstring": "cpp" } } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 6c68f01..ebdc672 100644 --- a/platformio.ini +++ b/platformio.ini @@ -18,3 +18,6 @@ lib_deps = bogde/HX711@^0.7.5 mathertel/OneButton@^2.0.3 sstaub/Ticker@^4.4.0 + robtillaart/CRC@^0.3.3 + sandeepmistry/CAN@^0.3.1 + fastled/FastLED@^3.5.0 diff --git a/src/Serialcommand.cpp b/src/Serialcommand.cpp index f63bcb6..7ba4ca7 100644 --- a/src/Serialcommand.cpp +++ b/src/Serialcommand.cpp @@ -1,6 +1,9 @@ #include "Serialcommand.h" #include "Arduino.h" - +#include "CRC.h" +#define HEAD_HI 0xA5 +#define HEAD_LI 0x5A +#define DEBUG_OUT false Serialcommand::Serialcommand() { // 连接控制器 @@ -12,8 +15,8 @@ Serialcommand::Serialcommand() CmdState = 0; bufferIndex = 0; recv_flag = false; - outBuffer[0] = 0xff; - outBuffer[1] = 0xff; + outBuffer[0] = HEAD_HI; + outBuffer[1] = HEAD_LI; } Serialcommand::~Serialcommand() { @@ -24,27 +27,27 @@ void Serialcommand::Uart_Command_Rev(void) while (Serial2.available() > 0) { ch = Serial2.read(); - // printf("rev:%d\n", ch); + if (DEBUG_OUT) printf("rev:%d\n", ch); switch (CmdState) { // 开始标志1 case 0: { - if (0xFF == ch) + if (HEAD_HI == ch) { CmdState = 1; - // printf("st:%d\n", CmdState); + if (DEBUG_OUT) printf("st:%d\n", CmdState); } } break; // 开始标志2 case 1: { - if (0xFF == ch) + if (HEAD_LI == ch) { bufferIndex = 0; CmdState = 2; - // printf("st:%d\n", CmdState); + if (DEBUG_OUT) printf("st:%d\n", CmdState); } else CmdState = 0; @@ -54,13 +57,13 @@ void Serialcommand::Uart_Command_Rev(void) case 2: { datalen = ch; - // printf("datalen:%d\n", datalen); + if (DEBUG_OUT) printf("datalen:%d\n", datalen); if (datalen <= inbuffersize) { CmdState = 3; bufferIndex = 0; inBuffer[bufferIndex] = ch; - // printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]); + if (DEBUG_OUT) printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]); } else // 超长 { @@ -71,13 +74,13 @@ void Serialcommand::Uart_Command_Rev(void) // 收数据直到结束 case 3: { - // printf("st3:%d:%d\n", bufferIndex, datalen); + if (DEBUG_OUT) printf("st3:%d:%d\n", bufferIndex, datalen); if (bufferIndex <= datalen) { bufferIndex++; inBuffer[bufferIndex] = ch; - // printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]); + if (DEBUG_OUT) printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]); if (bufferIndex == datalen) // 收完了 { CmdState = 0; @@ -85,22 +88,25 @@ void Serialcommand::Uart_Command_Rev(void) bufferIndex = 0; return; // 不收数据了，先出去解析 } + } else //异常 + { + if (DEBUG_OUT) printf("buf error:%d:%d\n", bufferIndex, datalen); + CmdState = 0; + bufferIndex = 0; + recv_flag=false; + return; } } break; } } } +//CRC8 +//多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107） +//https://zhuanlan.zhihu.com/p/114049042 uint8_t Serialcommand::getsum(const uint8_t *buffer, size_t size) { - uint16_t usum = 0; - uint8_t chksum = 0; - for (uint8_t i = 0; i < size; i++) - { - usum += buffer[i]; - } - chksum = (uint8_t)usum & 0xff; // 取低8位 - return chksum; + return crc8(buffer, size, 0x07, 0x00, 0x00, false, false); } bool Serialcommand::checksum() { @@ -148,13 +154,13 @@ void Serialcommand::getcommand() // 收到有效数据 if (recv_flag) { - // printf("rev end len:%d\n", datalen); + if (DEBUG_OUT) printf("rev end len:%d\n", datalen); recv_flag = false; // 处理数据 if (checksum()) { uint8_t commandid = inBuffer[1]; - printf("rev ok cid:0x%02X\n", commandid); + printf("rev command id:0x%02X\n", commandid); switch (commandid) { // 放钩 diff --git a/src/config.h b/src/config.h index b68d2c5..8c9a432 100644 --- a/src/config.h +++ b/src/config.h @@ -15,6 +15,9 @@ #define LOADCELL_DOUT_PIN 2 #define LOADCELL_SCK_PIN 4 /////////////////////////////////////////// +#define SPEED_UP_SLOW 4 //rmp 或者 弧度/秒 + + // 挂钩状态 enum HookStatus diff --git a/src/led.cpp b/src/led.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/led.h b/src/led.h deleted file mode 100644 index e69de29..0000000 diff --git a/src/moto.cpp b/src/moto.cpp new file mode 100644 index 0000000..b43c31b --- /dev/null +++ b/src/moto.cpp @@ -0,0 +1,167 @@ +#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; // 进入死区维持原值 +} diff --git a/src/moto.h b/src/moto.h new file mode 100644 index 0000000..3ae2882 --- /dev/null +++ b/src/moto.h @@ -0,0 +1,83 @@ +#pragma once +#include +#include +#include +#define MOTO_REVERSED 1 //正反转1为正转，-1为反转 +#define MOTO_REDUCTION 36 +#define MOTO_MAXANG 8192 //0-8191 +#define MOTO_MAXANG_HALF MOTO_MAXANG / 2 + + +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; + + 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); + +class moto +{ +private: + 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(); + void settarget(float new_target); + moto_measure_t getmotoinfo(); +}; diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp new file mode 100644 index 0000000..2b5256b --- /dev/null +++ b/src/motocontrol.cpp @@ -0,0 +1,168 @@ + +#include "Arduino.h" +#include "motocontrol.h" +#include "moto.h" + +#define DEBUG_OUT false +Motocontrol::Motocontrol() +{ + _controlstatus.is_instore = false; + _controlstatus.is_setzero = false; + _controlstatus.zero_cnt = 0; + _controlstatus.motostatus = MotoStatus::MS_Stop; + _controlstatus.speed_targe = SPEED_SLOW; + _check_targetlength = false; +} +Motocontrol::~Motocontrol() +{ +} + +bool Motocontrol::init() // 初始化 +{ + return _moto_dji.init(); +} +void Motocontrol::setspeed(float motospeed) // 设置速度 +{ + _controlstatus.speed_targe = motospeed; +} +void Motocontrol::down(float length) // 放线 +{ + printf("down:%.2f \n", length); + moto_run(MotoStatus::MS_Down, length); +} +void Motocontrol::up(float length) // 收线 +{ + printf("up:%.2f \n", length); + moto_run(MotoStatus::MS_Up, length); +} +void Motocontrol::stop() // 停止 +{ + _moto_dji.settarget(0.0f); + _controlstatus.motostatus = MS_Stop; + printf("stop \n"); +} +void Motocontrol::setlocked(bool locked) +{ + if (locked) + { + stop(); + setzero(); + } + _controlstatus.is_toplocked=locked; +} +void Motocontrol::setzero() // 设置0长度位置 +{ + // 记录0长度位置--以后需要保存到falsh里面 + _controlstatus.zero_cnt = getmotoinfo().output_round_cnt; + _controlstatus.is_setzero = true; +} +int16_t Motocontrol::getlength() // 得到长度 +{ + return 0; +} +void Motocontrol::checkstatus() // 检查状态，比如什么时候停 +{ + moto_measure_t mf = _moto_dji.getmotoinfo(); + // 已设置零点 + if (_controlstatus.is_setzero) + { + // 总放线长度 + float curr_length = abs(mf.output_round_cnt - _controlstatus.zero_cnt) * WHEEL_PERIMETER; + // 不能超ROPE_MAXLENGTH + if ((curr_length > ROPE_MAXLENGTH)&&(_controlstatus.motostatus == MS_Down)) + stop(); + // 开始入仓 + if ((curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up)) + { + _moto_dji.settarget(-SPEED_MIN); + _controlstatus.is_instore = true; + } + else + _controlstatus.is_instore = false; + } + if (_check_targetlength) // 有目标长度 + { + if (_controlstatus.motostatus == MS_Down) + { + if (mf.output_round_cnt > _target_cnt) + { + printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt); + stop(); + } + } + else if (_controlstatus.motostatus == MS_Up) + { + if (mf.output_round_cnt < _target_cnt) + { + printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt); + stop(); + } + } + } +} + +void Motocontrol::update() // 更新 +{ + _moto_dji.update(); + checkstatus(); +} +void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用 +{ + _moto_dji.settarget(motospeed); +} + +moto_measure_t Motocontrol::getmotoinfo() +{ + return _moto_dji.getmotoinfo(); +} +control_status_t Motocontrol::getcontrolstatus() // 得到控制信息 +{ + return _controlstatus; +} +// 按指定速度收放线 放线为+，收线为- 单位cm +void Motocontrol::moto_run(MotoStatus updown, float length) +{ + // 传入要stop，直接停 + if (updown == MS_Stop) + { + stop(); + return; + } + + // 运动中暂时不管 + if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up)) + return; + // 没设置速度不转 + if (_controlstatus.speed_targe == 0) + return; + + _controlstatus.motostatus = updown; + // 有长度限制 + if (length != 0) + { + // 收需要负 + if (updown == MotoStatus::MS_Up) + length = -length; + // 记录开始位置 + _start_cnt = _moto_dji.getmotoinfo().output_round_cnt; + _target_cnt = _start_cnt + length / WHEEL_PERIMETER; + _check_targetlength = true; + } + else + _check_targetlength = false; + + // 记录开始位置 + float runspeed; + if (updown == MotoStatus::MS_Down) + runspeed = _controlstatus.speed_targe; + else + { + // 如果没有设置零点，不能快速收线 + if (!_controlstatus.is_setzero) + _controlstatus.speed_targe = SPEED_SLOW; + runspeed = -_controlstatus.speed_targe; + } + // 开始转 + printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt); + _moto_dji.settarget(runspeed); +} \ No newline at end of file diff --git a/src/motocontrol.h b/src/motocontrol.h new file mode 100644 index 0000000..be63d14 --- /dev/null +++ b/src/motocontrol.h @@ -0,0 +1,70 @@ +#pragma once +#include "Arduino.h" +#include "config.h" +#include "moto.h" +#define ROPE_MAXLENGTH 600 //最多能放600cm---实际绳子应该比这个长650之类的 +#define WHEEL_DIAMETER 4.0 //轮子直径cm +#define INSTORE_LENGTH 15 //入仓长度,低于该长度要缓慢入仓 cm + +#define SPEED_MAX 330 //最快收放线速度 +#define SPEED_MIN 3 //入仓速度 +#define SPEED_SLOW 20 //慢收放线速度 +#define SPEED_FAST 100 //快收放线速度 +#define SPEED_BTN_FAST 200 //最快收放线速度 + +#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长 +#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数 + + + +// 挂钩状态 +enum MotoStatus +{ + MS_Down, // 自动下放中 + MS_Up, // 回收中 + MS_Stop //停止中 +}; + +typedef struct +{ + bool is_instore; + bool is_setzero; + bool is_toplocked; + float zero_cnt; + float speed_targe; + MotoStatus motostatus; + +} control_status_t; + + + + + +class Motocontrol +{ +private: + moto _moto_dji; + float _start_cnt; + float _target_cnt; + control_status_t _controlstatus; + bool _check_targetlength; + + void checkstatus(); +public: + Motocontrol(); // 构造函数 + ~Motocontrol(); // 析构函数 + void setspeed(float motospeed); // 设置速度 + void down(float length); // 收线 + void up(float length); // 放线 + void stop(); // 停止 + void setzero(); // 设置0长度位置 + int16_t getlength(); // 得到长度 + void update(); // 更新 + bool init(); // 初始化 + void direct_speed(float motospeed); //直接设置电机速度--立马生效,仅用于测试 + moto_measure_t getmotoinfo(); //得到电机信息 + control_status_t getcontrolstatus(); //得到控制信息 + void setlocked(bool locked); //是否到顶锁定 + void moto_run(MotoStatus updown,float length= 0.0f) ;// 放线 + +};