26d48db32b
【主 题】：代码格式化 较上版本 无任何变动 【描 述】： [原因]： [过程]： [影响]： 【结 束】 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
573 lines
16 KiB
C++
573 lines
16 KiB
C++
|
||
#include "Arduino.h"
|
||
#include "motocontrol.h"
|
||
#include "moto.h"
|
||
#include <ESP32Servo.h>
|
||
#include "config.h"
|
||
|
||
#define DEBUG_OUT false
|
||
static const char* MOUDLENAME = "MOTO_C";
|
||
|
||
Motocontrol::Motocontrol()
|
||
{
|
||
_controlstatus.is_instore = false;
|
||
_controlstatus.is_setzero = false;
|
||
_controlstatus.is_overweight = false;
|
||
_controlstatus.zero_cnt = 0;
|
||
_controlstatus.motostatus = MotoStatus::MS_Stop;
|
||
_controlstatus.speed_targe = SPEED_BTN_SLOW;
|
||
_check_targetlength = false;
|
||
_pullweight = 0;
|
||
_curr_length = 0.0;
|
||
_hooksstatus = HS_UnInit;
|
||
_weightalign = false;
|
||
_overweightcount = 0;
|
||
_notweightcount = 0;
|
||
_controlstatus.is_autogoodsdown=false;
|
||
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
|
||
_unblocktimes = 0;
|
||
}
|
||
Motocontrol::~Motocontrol()
|
||
{
|
||
}
|
||
|
||
bool Motocontrol::init(Servo *lockservo) // 初始化
|
||
{
|
||
_lockservo = lockservo;
|
||
// 不管堵转问题，马上会执行其他初始化让舵机介绍
|
||
_lockservo->write(SERVO_LOCKPOS);
|
||
_servotatus = SS_ServoLocked;
|
||
|
||
return _moto_dji.init();
|
||
}
|
||
void Motocontrol::setspeed(float motospeed, float acctime) // 设置速度
|
||
{
|
||
_controlstatus.speed_targe = motospeed;
|
||
_moto_dji.settime_acc(acctime);
|
||
}
|
||
|
||
void Motocontrol::setweight(int pullweight) // 设置重量
|
||
{
|
||
_pullweight = pullweight;
|
||
checkgoods();
|
||
}
|
||
String Motocontrol::gethooktatus_str(bool chstr)
|
||
{
|
||
String hookstatusstr="未知";
|
||
String hookstatusstr_en="unknown";
|
||
switch (_hooksstatus)
|
||
{
|
||
case HS_UnInit:
|
||
hookstatusstr="未初始化";
|
||
hookstatusstr_en="HS_UnInit";
|
||
break;
|
||
case HS_Down:
|
||
hookstatusstr="放钩";
|
||
hookstatusstr_en="HS_Down";
|
||
break;
|
||
case HS_DownSlow:
|
||
hookstatusstr="慢速放钩";
|
||
hookstatusstr_en="HS_DownSlow";
|
||
break;
|
||
case HS_WaitUnhook:
|
||
hookstatusstr="等待脱钩";
|
||
hookstatusstr_en="HS_WaitUnhook";
|
||
break;
|
||
case HS_Up:
|
||
hookstatusstr="回收";
|
||
hookstatusstr_en="HS_Up";
|
||
break;
|
||
case HS_InStore:
|
||
hookstatusstr="入仓中";
|
||
hookstatusstr_en="HS_InStore";
|
||
break;
|
||
case HS_Locked:
|
||
hookstatusstr="已锁定";
|
||
hookstatusstr_en="HS_Locked";
|
||
break;
|
||
case HS_Stop:
|
||
hookstatusstr="停止";
|
||
hookstatusstr_en="HS_Stop";
|
||
break;
|
||
default:
|
||
hookstatusstr="未知";
|
||
hookstatusstr_en="HS_UnInit";
|
||
break;
|
||
}
|
||
if (chstr)
|
||
return hookstatusstr;
|
||
else
|
||
return hookstatusstr_en;
|
||
|
||
|
||
}
|
||
|
||
void Motocontrol::checkgoods() // 检测是否超重
|
||
{
|
||
// 到顶部锁定状态，有向上的压力，重量不准，不能检测
|
||
if (_controlstatus.is_toplocked)
|
||
return;
|
||
|
||
// 检查是否超重
|
||
if (_pullweight > HOOK_WEIHT_MAX)
|
||
{
|
||
// 防止毛刺
|
||
_overweightcount++;
|
||
if (_overweightcount > 40)
|
||
_controlstatus.is_overweight = true;
|
||
}
|
||
else
|
||
{
|
||
_controlstatus.is_overweight = false;
|
||
_overweightcount = 0;
|
||
}
|
||
|
||
// 检查是否有货物
|
||
if (_pullweight < HOOK_WEIHT_MIN)
|
||
{
|
||
// 防止毛刺
|
||
_notweightcount++;
|
||
if (_notweightcount > 40)
|
||
{
|
||
// printf("goods weight<min 40 times :%d \n", _pullweight);
|
||
_controlstatus.is_havegoods = false;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
_controlstatus.is_havegoods = true;
|
||
_notweightcount = 0;
|
||
}
|
||
}
|
||
void Motocontrol::lockservo() // 锁定舵机
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"start_lockservo");
|
||
_servotatus = SS_WaitMotoStop;
|
||
_tm_servotatus = millis();
|
||
}
|
||
void Motocontrol::unlockservo() // 解锁舵机
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"unlockservo");
|
||
// 解锁操作
|
||
_lockservo->write(SERVO_UNLOCKPOS);
|
||
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
|
||
_servotatus = SS_WaitUnLock;
|
||
_tm_servotatus = millis();
|
||
_unblocktimes = 0;
|
||
}
|
||
void Motocontrol::stoprun(float acctime) // 停止
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime);
|
||
_moto_dji.settime_acc(acctime);
|
||
_moto_dji.setspeedtarget(0.0f);
|
||
_controlstatus.motostatus = MS_Stop;
|
||
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) )
|
||
{
|
||
_hooksstatus_prv = _hooksstatus;
|
||
sethooksstatus(HS_Stop);
|
||
}
|
||
lockservo();
|
||
}
|
||
void Motocontrol::stopautodown()
|
||
{
|
||
_controlstatus.is_autogoodsdown = false;
|
||
}
|
||
void Motocontrol::sethooksstatus(HookStatus hookstatus)
|
||
{
|
||
_hooksstatus=hookstatus;
|
||
ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false));
|
||
}
|
||
void Motocontrol::setlocked(bool locked)
|
||
{
|
||
if (locked)
|
||
{
|
||
stoprun();
|
||
setzero();
|
||
sethooksstatus(HS_Locked);
|
||
_controlstatus.is_autogoodsdown = false;
|
||
}
|
||
_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::weightalign(bool weightalign)
|
||
{
|
||
_weightalign = weightalign;
|
||
}
|
||
void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||
{
|
||
// 需要在自动放货物中
|
||
if (!_controlstatus.is_autogoodsdown)
|
||
return;
|
||
moto_measure_t mf = _moto_dji.getmotoinfo();
|
||
_hook_currlen = _curr_length;
|
||
switch (_hooksstatus)
|
||
{
|
||
// 正在下放货物
|
||
case HookStatus::HS_Down:
|
||
{
|
||
// 如果没有货物--开始回收
|
||
if (!_controlstatus.is_havegoods)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME);
|
||
sethooksstatus(HS_WaitUnhook);
|
||
_tm_waitunhook = millis();
|
||
break;
|
||
}
|
||
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt);
|
||
sethooksstatus(HS_DownSlow);
|
||
_moto_dji.settime_acc(500);
|
||
set_hook_speed(SPEED_HOOK_SLOW);
|
||
}
|
||
break;
|
||
}
|
||
// 开始慢速下放
|
||
case HookStatus::HS_DownSlow:
|
||
{
|
||
if (!_controlstatus.is_havegoods)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME);
|
||
sethooksstatus(HS_WaitUnhook);
|
||
_tm_waitunhook = millis();
|
||
}
|
||
break;
|
||
}
|
||
// 等待脱钩
|
||
case HookStatus::HS_WaitUnhook:
|
||
{
|
||
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup");
|
||
_moto_dji.settime_acc(1000);
|
||
set_hook_speed(-SPEED_HOOK_UP);
|
||
sethooksstatus(HS_Up);
|
||
_controlstatus.motostatus = MS_Up;
|
||
}
|
||
break;
|
||
}
|
||
// 开始收回
|
||
case HookStatus::HS_Up:
|
||
{
|
||
// 会自动设置HS_Up状态
|
||
break;
|
||
}
|
||
case HookStatus::HS_InStore:
|
||
{
|
||
// 会自动设置HS_InStore状态
|
||
// 如果到顶部，会触发setlocked,不用在这里设置
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
// 检测舵机堵转---电流大或者角度不够，需要传感器
|
||
bool Motocontrol::checkservoblock()
|
||
{
|
||
return false;
|
||
}
|
||
void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候停
|
||
{
|
||
moto_measure_t mf = _moto_dji.getmotoinfo();
|
||
|
||
// 停止状态---1防止舵机堵转，2延时关闭电机供电
|
||
switch (_servotatus)
|
||
{
|
||
case SS_WaitUnLock: // 舵机解锁
|
||
{
|
||
if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
|
||
{
|
||
// 入仓时会自动设置速度，原速度不可用
|
||
if (!_controlstatus.is_instore)
|
||
_moto_dji.setspeedtarget(_runspeed);
|
||
_servotatus = SS_ServoUnLocked;
|
||
}
|
||
break;
|
||
}
|
||
|
||
// 等待电机停止
|
||
case SS_WaitMotoStop:
|
||
{
|
||
if (abs(mf.speed_rpm) < 3)
|
||
{
|
||
_lockservo->write(SERVO_LOCKPOS);
|
||
_servotatus = SS_WaitServo;
|
||
_tm_servotatus = millis();
|
||
}
|
||
break;
|
||
}
|
||
// 等待舵机执行到位---------防止堵转，功能没有使用，采用硬件防堵转舵机
|
||
case SS_WaitServo:
|
||
{
|
||
// 舵机执行时间
|
||
if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
|
||
{
|
||
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
|
||
if (!checkservoblock() || (_unblocktimes > 2))
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"Close moto");
|
||
_moto_dji.close();
|
||
_servotatus = SS_ServoLocked;
|
||
}
|
||
else
|
||
// 堵转
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto");
|
||
_tm_servotatus = millis();
|
||
_servotatus = SS_WaitUnBlock;
|
||
// 写一个不会堵转的位置
|
||
_lockservo->write(SERVO_BLOCKUNLOCKPOS);
|
||
// 转一点电机角度
|
||
_moto_dji.setspeedtarget(SPEED_UNBLOCK);
|
||
}
|
||
}
|
||
|
||
break;
|
||
}
|
||
case SS_WaitUnBlock:
|
||
{
|
||
if ((millis() - _tm_servotatus) > TM_UNBLOCK)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"SS lock servo");
|
||
// 继续锁定等待舵机检测
|
||
_lockservo->write(SERVO_LOCKPOS);
|
||
_servotatus = SS_WaitServo;
|
||
_tm_servotatus = millis();
|
||
_unblocktimes++;
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 已设置零点
|
||
if (_controlstatus.is_setzero)
|
||
{
|
||
// 总放线长度
|
||
_curr_length = abs(mf.output_round_cnt - _controlstatus.zero_cnt) * WHEEL_PERIMETER;
|
||
// 不能超ROPE_MAXLENGTH
|
||
if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
|
||
stoprun();
|
||
// 开始入仓
|
||
// if ((_curr_length < (INSTORE_LENGTH_MIN + (mf.output_speed_rpm * mf.output_speed_rpm) * INSTORE_LENGTH_SPEED)) && (_controlstatus.motostatus == MotoStatus::MS_Up))
|
||
float instlen;
|
||
if (_controlstatus.is_havegoods)
|
||
instlen = INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
|
||
else
|
||
instlen = INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
|
||
if ((_curr_length < instlen) && (_controlstatus.motostatus == MotoStatus::MS_Up))
|
||
{
|
||
if (!_controlstatus.is_instore)
|
||
{
|
||
sethooksstatus(HS_InStore);
|
||
_moto_dji.settime_acc(500);
|
||
set_hook_speed(-SPEED_INSTORE);
|
||
_controlstatus.is_instore = true;
|
||
ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
|
||
if (_hooksstatus != HS_InStore)
|
||
_controlstatus.is_instore = false;
|
||
}
|
||
}
|
||
if (_check_targetlength) // 有目标长度
|
||
{
|
||
if (_controlstatus.motostatus == MS_Down)
|
||
{
|
||
if (mf.output_round_cnt > _target_cnt)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
|
||
stoprun(1000); // 缓慢停止
|
||
}
|
||
}
|
||
else if (_controlstatus.motostatus == MS_Up)
|
||
{
|
||
if (mf.output_round_cnt < _target_cnt)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
|
||
stoprun(1000); // 缓慢停止
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void Motocontrol::set_hook_speed(float hooksspeed)
|
||
{
|
||
_goods_speed = hooksspeed;
|
||
_runspeed = hooksspeed;
|
||
_moto_dji.setspeedtarget(hooksspeed);
|
||
}
|
||
void Motocontrol::update() // 更新
|
||
{
|
||
_moto_dji.update();
|
||
checkmotostatus();
|
||
checkhookstatus();
|
||
}
|
||
void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
|
||
{
|
||
_moto_dji.setspeedtarget(motospeed);
|
||
}
|
||
|
||
moto_measure_t Motocontrol::getmotoinfo()
|
||
{
|
||
return _moto_dji.getmotoinfo();
|
||
}
|
||
control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
|
||
{
|
||
return _controlstatus;
|
||
}
|
||
|
||
void Motocontrol::moto_goodsdownresume()
|
||
{
|
||
if (!_controlstatus.is_autogoodsdown) return;
|
||
if (_hooksstatus == HS_Stop)
|
||
{
|
||
_runspeed = _goods_speed;
|
||
if (_hooksstatus_prv == HS_Up)
|
||
_controlstatus.motostatus = MS_Up;
|
||
if (_hooksstatus_prv == HS_InStore)
|
||
{
|
||
_controlstatus.motostatus = MS_Up;
|
||
_runspeed = -SPEED_INSTORE;
|
||
}
|
||
sethooksstatus(_hooksstatus_prv);
|
||
_moto_dji.settime_acc(1000);
|
||
unlockservo(); // 打开舵机
|
||
// _moto_dji.setspeedtarget(_goods_speed);
|
||
}
|
||
}
|
||
//长度cm
|
||
void Motocontrol::moto_goodsdown(float length)
|
||
{
|
||
if (length < 0.0)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"length<0 fault");
|
||
return;
|
||
}
|
||
// 没设置零点
|
||
if (!_controlstatus.is_setzero)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"not lengthzero fault");
|
||
return;
|
||
}
|
||
if (!_weightalign)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"weight not align fault");
|
||
return;
|
||
}
|
||
// 没挂东西
|
||
if (!_controlstatus.is_havegoods)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight);
|
||
return;
|
||
}
|
||
|
||
// 开始自动放货物状态
|
||
_controlstatus.is_autogoodsdown = true;
|
||
// 长度=0，直接中慢速下降，
|
||
if (length == 0.0)
|
||
|
||
{
|
||
_goods_down_target_cnt = 0;
|
||
setspeed(SPEED_HOOK_CHECK);
|
||
_goods_speed = SPEED_HOOK_CHECK;
|
||
}
|
||
else
|
||
{
|
||
float tarleng=length;
|
||
if (length>HOOK_SLOWDOWN_LENGTH)
|
||
tarleng-=HOOK_SLOWDOWN_LENGTH;
|
||
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
|
||
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
|
||
ESP_LOGD(MOUDLENAME,"start down %.2f cm,tar:%.2f",tarleng,_goods_down_target_cnt);
|
||
setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
|
||
_goods_speed = SPEED_HOOK_FAST;
|
||
}
|
||
moto_run(MS_Down);
|
||
}
|
||
|
||
// 按指定速度收放线 放线为+，收线为- 单位cm
|
||
void Motocontrol::moto_run(MotoStatus updown, float length)
|
||
{
|
||
// 传入要stop，直接停
|
||
if (updown == MS_Stop)
|
||
{
|
||
stoprun();
|
||
return;
|
||
}
|
||
|
||
// 运动中暂时不管
|
||
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up");
|
||
return;
|
||
}
|
||
// 没设置速度不转
|
||
if (_controlstatus.speed_targe == 0)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"not set speed_targe");
|
||
return;
|
||
}
|
||
if (updown == MS_Up)
|
||
{
|
||
if (_controlstatus.is_toplocked)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"is_toplocked return");
|
||
return;
|
||
}
|
||
if (_controlstatus.is_overweight)
|
||
{
|
||
ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight);
|
||
return;
|
||
}
|
||
sethooksstatus(HS_Up);
|
||
}
|
||
else
|
||
sethooksstatus(HS_Down);
|
||
|
||
unlockservo();
|
||
|
||
_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_BTN_SLOW;
|
||
runspeed = -_controlstatus.speed_targe;
|
||
}
|
||
// 开始转
|
||
// ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
|
||
_runspeed = runspeed;
|
||
// _moto_dji.setspeedtarget(runspeed);
|
||
} |