Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

【类 型】：style

Browse Source 
【主	题】：代码格式化 较上版本 无任何变动
【描	述】：
	[原因]：
	[过程]：
	[影响]：
【结	束】

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
This commit is contained in:
tk 2024-07-02 19:36:54 +08:00
parent 811a89f8b2
commit 26d48db32b
5 changed files with 115 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/config.h
View File

@ -3,8 +3,8 @@
// 定义公共结构，变量，硬件接口等 // 定义公共结构，变量，硬件接口等
/// ///
// //
#define VERSION "0.90" // 软件版本 #define VERSION "0.90" //软件版本
#define VERSION_HW 1 // 硬件版本1:第一块硬件 2:目前版本 #define VERSION_HW 1 //硬件版本1:第一块硬件 2:目前版本
// 硬件接口定义//////////////////////////// // 硬件接口定义////////////////////////////
// 按钮 // 按钮
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND #define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
@ -12,36 +12,37 @@
#define BTN_CT 21 // 到顶检测开关 #define BTN_CT 21 // 到顶检测开关
#define BTN_TEST 18 // 测试开关 #define BTN_TEST 18 // 测试开关
// 称重传感器- HX711 // 称重传感器- HX711
#define LOADCELL_DOUT_PIN 13 // 16 #define LOADCELL_DOUT_PIN 13 //16
#define LOADCELL_SCK_PIN 33 // 17 #define LOADCELL_SCK_PIN 33 //17
/////////////////////////////////////////// ///////////////////////////////////////////
#define SERVO_PIN 14 // 锁定舵机PWM控制脚 #define SERVO_PIN 14 // 锁定舵机PWM控制脚
////LED ////LED
#define LED_DATA_PIN 25 #define LED_DATA_PIN 25
#if (VERSION_HW == 1) #if (VERSION_HW == 1)
// Moto-CAN //第一版本硬件参数---1号机使用 // Moto-CAN //第一版本硬件参数---1号机使用
#define MOTO_CAN_RX 26 #define MOTO_CAN_RX 26
#define MOTO_CAN_TX 27 #define MOTO_CAN_TX 27
#define WEIGHT_SCALE 165 // //A通道是165,B通道是41 #define WEIGHT_SCALE 165 // //A通道是165,B通道是41
#define HX711_GAIN 128 #define HX711_GAIN 128
#else #else
// Moto-CAN //第二版本硬件参数---2号机使用 // Moto-CAN //第二版本硬件参数---2号机使用
#define MOTO_CAN_RX 27 // PCB画板需要，做了调整 #define MOTO_CAN_RX 27 //PCB画板需要，做了调整
#define MOTO_CAN_TX 26 // PCB画板需要，做了调整 #define MOTO_CAN_TX 26 //PCB画板需要，做了调整
#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41 #define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41
#define HX711_GAIN 32 // 减少零点漂移用B通道的感度 #define HX711_GAIN 32 //减少零点漂移用B通道的感度
#endif #endif
/// serial1 ///serial1
#define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_TX 5
#define SERIAL_REPORT_RX 18 #define SERIAL_REPORT_RX 18
///// /////
// #define WEIGHT_SCALE 41 // //A通道是165,B通道是41 //#define WEIGHT_SCALE 41 // //A通道是165,B通道是41
#define TM_INSTORE_WEIGHT_DELAY 200 // 200 // 入仓动力延时关闭时间 ms #define TM_INSTORE_WEIGHT_DELAY 200 //200 // 入仓动力延时关闭时间 ms
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms #define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms #define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
enum HookStatus enum HookStatus
{ {
HS_UnInit, // 还未初始化 HS_UnInit, // 还未初始化
@ -66,9 +67,9 @@ enum Initstatus
IS_CheckWeightZero, // 检查称重传感器是否归0 IS_CheckWeightZero, // 检查称重传感器是否归0
IS_OK, // 4.所有初始化已经OK，可以正常操作 IS_OK, // 4.所有初始化已经OK，可以正常操作
IS_Error // 5.初始化遇到错误 IS_Error // 5.初始化遇到错误
}; } ;
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN = 40; // 飞控发的---自动放线 const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线
const uint16_t MAV_CMD_FC_HOOK_PAUSE = 41; // 飞控发的---暂停 const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停
const uint16_t MAV_CMD_FC_HOOK_STATUS = 42; // 发给飞控的状态 const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态
const uint16_t MAV_CMD_FC_HOOK_RECOVERY = 43; // 飞控发的---收线 const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
const uint16_t MAV_CMD_FC_DESCENTSTART = 44; // 飞开始下降，需要调整相机向下 const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下

2
src/main.cpp
View File

@ -804,5 +804,3 @@ void testbtn_click()
} }
} }
} }
//////////////////////////////MQTT_语音_MAVLINK 部分

8
src/moto.cpp
View File

@ -5,20 +5,20 @@
uint8_t CaninBuffer[8]; // 接收指令缓冲区 uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis; moto_measure_t moto_chassis;
PID_TypeDef moto_pid; PID_TypeDef moto_pid;
static const char *MOUDLENAME = "MOTO"; static const char* MOUDLENAME = "MOTO";
moto::moto() moto::moto()
{ {
_closed = true; _closed = true;
} }
bool moto::init() bool moto::init()
{ {
ESP_LOGD(MOUDLENAME, "init moto"); ESP_LOGD(MOUDLENAME,"init moto");
pid_init(); pid_init();
CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
// start the CAN bus at 500 kbps // start the CAN bus at 500 kbps
if (!CAN.begin(1000E3)) if (!CAN.begin(1000E3))
{ {
ESP_LOGE(MOUDLENAME, "Starting CAN failed!"); ESP_LOGE(MOUDLENAME,"Starting CAN failed!");
return false; return false;
} }
CAN.onReceive(onReceive); CAN.onReceive(onReceive);
@ -30,7 +30,7 @@ moto::~moto()
void moto::update() void moto::update()
{ {
// printf("u1:%d\n",_closed); //printf("u1:%d\n",_closed);
if (!_closed) if (!_closed)
{ {
unsigned long dt = millis() - moto_chassis.starttime; // 时间差 unsigned long dt = millis() - moto_chassis.starttime; // 时间差

110
src/motocontrol.cpp
View File

@ -6,7 +6,7 @@
#include "config.h" #include "config.h"
#define DEBUG_OUT false #define DEBUG_OUT false
static const char *MOUDLENAME = "MOTO_C"; static const char* MOUDLENAME = "MOTO_C";
Motocontrol::Motocontrol() Motocontrol::Motocontrol()
{ {
@ -23,7 +23,7 @@ Motocontrol::Motocontrol()
_weightalign = false; _weightalign = false;
_overweightcount = 0; _overweightcount = 0;
_notweightcount = 0; _notweightcount = 0;
_controlstatus.is_autogoodsdown = false; _controlstatus.is_autogoodsdown=false;
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机 _servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
_unblocktimes = 0; _unblocktimes = 0;
} }
@ -53,51 +53,53 @@ void Motocontrol::setweight(int pullweight) // 设置重量
} }
String Motocontrol::gethooktatus_str(bool chstr) String Motocontrol::gethooktatus_str(bool chstr)
{ {
String hookstatusstr = "未知"; String hookstatusstr="未知";
String hookstatusstr_en = "unknown"; String hookstatusstr_en="unknown";
switch (_hooksstatus) switch (_hooksstatus)
{ {
case HS_UnInit: case HS_UnInit:
hookstatusstr = "未初始化"; hookstatusstr="未初始化";
hookstatusstr_en = "HS_UnInit"; hookstatusstr_en="HS_UnInit";
break; break;
case HS_Down: case HS_Down:
hookstatusstr = "放钩"; hookstatusstr="放钩";
hookstatusstr_en = "HS_Down"; hookstatusstr_en="HS_Down";
break; break;
case HS_DownSlow: case HS_DownSlow:
hookstatusstr = "慢速放钩"; hookstatusstr="慢速放钩";
hookstatusstr_en = "HS_DownSlow"; hookstatusstr_en="HS_DownSlow";
break; break;
case HS_WaitUnhook: case HS_WaitUnhook:
hookstatusstr = "等待脱钩"; hookstatusstr="等待脱钩";
hookstatusstr_en = "HS_WaitUnhook"; hookstatusstr_en="HS_WaitUnhook";
break; break;
case HS_Up: case HS_Up:
hookstatusstr = "回收"; hookstatusstr="回收";
hookstatusstr_en = "HS_Up"; hookstatusstr_en="HS_Up";
break; break;
case HS_InStore: case HS_InStore:
hookstatusstr = "入仓中"; hookstatusstr="入仓中";
hookstatusstr_en = "HS_InStore"; hookstatusstr_en="HS_InStore";
break; break;
case HS_Locked: case HS_Locked:
hookstatusstr = "已锁定"; hookstatusstr="已锁定";
hookstatusstr_en = "HS_Locked"; hookstatusstr_en="HS_Locked";
break; break;
case HS_Stop: case HS_Stop:
hookstatusstr = "停止"; hookstatusstr="停止";
hookstatusstr_en = "HS_Stop"; hookstatusstr_en="HS_Stop";
break; break;
default: default:
hookstatusstr = "未知"; hookstatusstr="未知";
hookstatusstr_en = "HS_UnInit"; hookstatusstr_en="HS_UnInit";
break; break;
} }
if (chstr) if (chstr)
return hookstatusstr; return hookstatusstr;
else else
return hookstatusstr_en; return hookstatusstr_en;
} }
void Motocontrol::checkgoods() // 检测是否超重 void Motocontrol::checkgoods() // 检测是否超重
@ -139,13 +141,13 @@ void Motocontrol::checkgoods() // 检测是否超重
} }
void Motocontrol::lockservo() // 锁定舵机 void Motocontrol::lockservo() // 锁定舵机
{ {
ESP_LOGD(MOUDLENAME, "start_lockservo"); ESP_LOGD(MOUDLENAME,"start_lockservo");
_servotatus = SS_WaitMotoStop; _servotatus = SS_WaitMotoStop;
_tm_servotatus = millis(); _tm_servotatus = millis();
} }
void Motocontrol::unlockservo() // 解锁舵机 void Motocontrol::unlockservo() // 解锁舵机
{ {
ESP_LOGD(MOUDLENAME, "unlockservo"); ESP_LOGD(MOUDLENAME,"unlockservo");
// 解锁操作 // 解锁操作
_lockservo->write(SERVO_UNLOCKPOS); _lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
@ -155,11 +157,11 @@ void Motocontrol::unlockservo() // 解锁舵机
} }
void Motocontrol::stoprun(float acctime) // 停止 void Motocontrol::stoprun(float acctime) // 停止
{ {
ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", acctime); ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime);
_moto_dji.settime_acc(acctime); _moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f); _moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop; _controlstatus.motostatus = MS_Stop;
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) && (_hooksstatus != HS_Locked)) if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) )
{ {
_hooksstatus_prv = _hooksstatus; _hooksstatus_prv = _hooksstatus;
sethooksstatus(HS_Stop); sethooksstatus(HS_Stop);
@ -172,8 +174,8 @@ void Motocontrol::stopautodown()
} }
void Motocontrol::sethooksstatus(HookStatus hookstatus) void Motocontrol::sethooksstatus(HookStatus hookstatus)
{ {
_hooksstatus = hookstatus; _hooksstatus=hookstatus;
ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false)); ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false));
} }
void Motocontrol::setlocked(bool locked) void Motocontrol::setlocked(bool locked)
{ {
@ -197,6 +199,7 @@ int16_t Motocontrol::getlength() // 得到长度
return 0; return 0;
} }
// 重量传感器已经校准 // 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign) void Motocontrol::weightalign(bool weightalign)
{ {
@ -217,14 +220,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 如果没有货物--开始回收 // 如果没有货物--开始回收
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME, "Not goods wait %d ms for unhook", HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
break; break;
} }
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt)) 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); ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt);
sethooksstatus(HS_DownSlow); sethooksstatus(HS_DownSlow);
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(SPEED_HOOK_SLOW); set_hook_speed(SPEED_HOOK_SLOW);
@ -236,7 +239,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME, "Not havegoods wait %d ms unhook", HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
} }
@ -247,7 +250,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{ {
ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup"); ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup");
_moto_dji.settime_acc(1000); _moto_dji.settime_acc(1000);
set_hook_speed(-SPEED_HOOK_UP); set_hook_speed(-SPEED_HOOK_UP);
sethooksstatus(HS_Up); sethooksstatus(HS_Up);
@ -313,14 +316,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
if (!checkservoblock() || (_unblocktimes > 2)) if (!checkservoblock() || (_unblocktimes > 2))
{ {
ESP_LOGD(MOUDLENAME, "Close moto"); ESP_LOGD(MOUDLENAME,"Close moto");
_moto_dji.close(); _moto_dji.close();
_servotatus = SS_ServoLocked; _servotatus = SS_ServoLocked;
} }
else else
// 堵转 // 堵转
{ {
ESP_LOGD(MOUDLENAME, "SS servoblock unlock servo and turn moto"); ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto");
_tm_servotatus = millis(); _tm_servotatus = millis();
_servotatus = SS_WaitUnBlock; _servotatus = SS_WaitUnBlock;
// 写一个不会堵转的位置 // 写一个不会堵转的位置
@ -336,7 +339,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if ((millis() - _tm_servotatus) > TM_UNBLOCK) if ((millis() - _tm_servotatus) > TM_UNBLOCK)
{ {
ESP_LOGD(MOUDLENAME, "SS lock servo"); ESP_LOGD(MOUDLENAME,"SS lock servo");
// 继续锁定等待舵机检测 // 继续锁定等待舵机检测
_lockservo->write(SERVO_LOCKPOS); _lockservo->write(SERVO_LOCKPOS);
_servotatus = SS_WaitServo; _servotatus = SS_WaitServo;
@ -370,12 +373,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(-SPEED_INSTORE); set_hook_speed(-SPEED_INSTORE);
_controlstatus.is_instore = true; _controlstatus.is_instore = true;
ESP_LOGD(MOUDLENAME, "Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm); ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
} }
} }
else else
{ {
// 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支 //已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
if (_hooksstatus != HS_InStore) if (_hooksstatus != HS_InStore)
_controlstatus.is_instore = false; _controlstatus.is_instore = false;
} }
@ -386,7 +389,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt > _target_cnt) if (mf.output_round_cnt > _target_cnt)
{ {
ESP_LOGD(MOUDLENAME, "stop down tar:%.2f", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -394,7 +397,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt < _target_cnt) if (mf.output_round_cnt < _target_cnt)
{ {
ESP_LOGD(MOUDLENAME, "stop up tar:%.2f", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -429,8 +432,7 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdownresume() void Motocontrol::moto_goodsdownresume()
{ {
if (!_controlstatus.is_autogoodsdown) if (!_controlstatus.is_autogoodsdown) return;
return;
if (_hooksstatus == HS_Stop) if (_hooksstatus == HS_Stop)
{ {
_runspeed = _goods_speed; _runspeed = _goods_speed;
@ -447,29 +449,29 @@ void Motocontrol::moto_goodsdownresume()
// _moto_dji.setspeedtarget(_goods_speed); // _moto_dji.setspeedtarget(_goods_speed);
} }
} }
// 长度cm //长度cm
void Motocontrol::moto_goodsdown(float length) void Motocontrol::moto_goodsdown(float length)
{ {
if (length < 0.0) if (length < 0.0)
{ {
ESP_LOGD(MOUDLENAME, "length<0 fault"); ESP_LOGD(MOUDLENAME,"length<0 fault");
return; return;
} }
// 没设置零点 // 没设置零点
if (!_controlstatus.is_setzero) if (!_controlstatus.is_setzero)
{ {
ESP_LOGD(MOUDLENAME, "not lengthzero fault"); ESP_LOGD(MOUDLENAME,"not lengthzero fault");
return; return;
} }
if (!_weightalign) if (!_weightalign)
{ {
ESP_LOGD(MOUDLENAME, "weight not align fault"); ESP_LOGD(MOUDLENAME,"weight not align fault");
return; return;
} }
// 没挂东西 // 没挂东西
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight); ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight);
return; return;
} }
@ -485,12 +487,12 @@ void Motocontrol::moto_goodsdown(float length)
} }
else else
{ {
float tarleng = length; float tarleng=length;
if (length > HOOK_SLOWDOWN_LENGTH) if (length>HOOK_SLOWDOWN_LENGTH)
tarleng -= HOOK_SLOWDOWN_LENGTH; tarleng-=HOOK_SLOWDOWN_LENGTH;
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt; _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER; _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); ESP_LOGD(MOUDLENAME,"start down %.2f cm,tar:%.2f",tarleng,_goods_down_target_cnt);
setspeed(SPEED_HOOK_FAST, TM_ACC_HS); setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST; _goods_speed = SPEED_HOOK_FAST;
} }
@ -510,25 +512,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 运动中暂时不管 // 运动中暂时不管
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up)) if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
{ {
ESP_LOGD(MOUDLENAME, "motostatus is MS_Down\\MS_Up"); ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up");
return; return;
} }
// 没设置速度不转 // 没设置速度不转
if (_controlstatus.speed_targe == 0) if (_controlstatus.speed_targe == 0)
{ {
ESP_LOGD(MOUDLENAME, "not set speed_targe"); ESP_LOGD(MOUDLENAME,"not set speed_targe");
return; return;
} }
if (updown == MS_Up) if (updown == MS_Up)
{ {
if (_controlstatus.is_toplocked) if (_controlstatus.is_toplocked)
{ {
ESP_LOGD(MOUDLENAME, "is_toplocked return"); ESP_LOGD(MOUDLENAME,"is_toplocked return");
return; return;
} }
if (_controlstatus.is_overweight) if (_controlstatus.is_overweight)
{ {
ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight); ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight);
return; return;
} }
sethooksstatus(HS_Up); sethooksstatus(HS_Up);

11
src/motocontrol.h
View File

@ -29,8 +29,8 @@
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms #define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
#define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些 #define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
#define SERVO_LOCKPOS 1000 // 1920 // 舵机锁定位置 #define SERVO_LOCKPOS 1000 //1920 // 舵机锁定位置
#define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置 #define SERVO_UNLOCKPOS 1120 //1800 // 舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 #define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克) #define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
@ -55,7 +55,7 @@ typedef struct
bool is_toplocked; // 已到顶锁住 bool is_toplocked; // 已到顶锁住
bool is_overweight; // 是否超重 bool is_overweight; // 是否超重
bool is_havegoods; // 是否有货物 bool is_havegoods; // 是否有货物
bool is_autogoodsdown; // 正在自动放线中 bool is_autogoodsdown; //正在自动放线中
float zero_cnt; // 0长度位置 float zero_cnt; // 0长度位置
float speed_targe; // 当前目标速度 float speed_targe; // 当前目标速度
MotoStatus motostatus; // 电机运行状态 MotoStatus motostatus; // 电机运行状态
@ -111,13 +111,12 @@ private:
void unlockservo(); void unlockservo();
void set_hook_speed(float hooksspeed); void set_hook_speed(float hooksspeed);
void sethooksstatus(HookStatus hookstatus); void sethooksstatus(HookStatus hookstatus);
public: public:
Motocontrol(); // 构造函数 Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数 ~Motocontrol(); // 析构函数
void setspeed(float motospeed, float acctime = -1); // 设置速度 void setspeed(float motospeed, float acctime = -1); // 设置速度
void stoprun(float acctime = 0); // 停止运行 void stoprun(float acctime = 0); // 停止运行
void stopautodown(); // 停止自动下放模式 void stopautodown(); //停止自动下放模式
void setzero(); // 设置0长度位置 void setzero(); // 设置0长度位置
int16_t getlength(); // 得到长度 int16_t getlength(); // 得到长度
void update(); // 更新 void update(); // 更新
@ -131,7 +130,7 @@ public:
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收 void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
void moto_goodsdownresume(); // 恢复自动放线 void moto_goodsdownresume(); // 恢复自动放线
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
String gethooktatus_str(bool chstr = true); // 得到挂钩状态(中英文） String gethooktatus_str(bool chstr=true); // 得到挂钩状态(中英文）
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
}; };