可以控制电机，彩色LED，和按钮的逻辑关系
问题：采用速度环控制，电机力量不够，需要改为位置环
This commit is contained in:
parent
bff743a387
commit
b599454f0c
10
.vscode/settings.json
vendored
10
.vscode/settings.json
vendored
@ -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"
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
{
|
||||
// 放钩
|
||||
|
||||
@ -15,6 +15,9 @@
|
||||
#define LOADCELL_DOUT_PIN 2
|
||||
#define LOADCELL_SCK_PIN 4
|
||||
///////////////////////////////////////////
|
||||
#define SPEED_UP_SLOW 4 //rmp 或者 弧度/秒
|
||||
|
||||
|
||||
|
||||
// 挂钩状态
|
||||
enum HookStatus
|
||||
|
||||
167
src/moto.cpp
Normal file
167
src/moto.cpp
Normal file
@ -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; // 进入死区维持原值
|
||||
}
|
||||
83
src/moto.h
Normal file
83
src/moto.h
Normal file
@ -0,0 +1,83 @@
|
||||
#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
|
||||
|
||||
|
||||
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();
|
||||
};
|
||||
168
src/motocontrol.cpp
Normal file
168
src/motocontrol.cpp
Normal file
@ -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);
|
||||
}
|
||||
70
src/motocontrol.h
Normal file
70
src/motocontrol.h
Normal file
@ -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) ;// 放线
|
||||
|
||||
};
|
||||
Loading…
Reference in New Issue
Block a user