【类 型】：style
【主 题】：代码格式化 较上版本 无任何变动 【描 述】： [原因]： [过程]： [影响]： 【结 束】 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
This commit is contained in:
tk
parent
811a89f8b2
commit
26d48db32b
49
src/config.h
49
src/config.h
@ -3,8 +3,8 @@
|
||||
// 定义公共结构，变量，硬件接口等
|
||||
///
|
||||
//
|
||||
#define VERSION "0.90" // 软件版本
|
||||
#define VERSION_HW 1 // 硬件版本1:第一块硬件 2:目前版本
|
||||
#define VERSION "0.90" //软件版本
|
||||
#define VERSION_HW 1 //硬件版本1:第一块硬件 2:目前版本
|
||||
// 硬件接口定义////////////////////////////
|
||||
// 按钮
|
||||
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
|
||||
@ -12,36 +12,37 @@
|
||||
#define BTN_CT 21 // 到顶检测开关
|
||||
#define BTN_TEST 18 // 测试开关
|
||||
// 称重传感器- HX711
|
||||
#define LOADCELL_DOUT_PIN 13 // 16
|
||||
#define LOADCELL_SCK_PIN 33 // 17
|
||||
#define LOADCELL_DOUT_PIN 13 //16
|
||||
#define LOADCELL_SCK_PIN 33 //17
|
||||
///////////////////////////////////////////
|
||||
#define SERVO_PIN 14 // 锁定舵机PWM控制脚
|
||||
////LED
|
||||
#define LED_DATA_PIN 25
|
||||
|
||||
#if (VERSION_HW == 1)
|
||||
// Moto-CAN //第一版本硬件参数---1号机使用
|
||||
#define MOTO_CAN_RX 26
|
||||
#define MOTO_CAN_TX 27
|
||||
#define WEIGHT_SCALE 165 // //A通道是165,B通道是41
|
||||
#define HX711_GAIN 128
|
||||
// Moto-CAN //第一版本硬件参数---1号机使用
|
||||
#define MOTO_CAN_RX 26
|
||||
#define MOTO_CAN_TX 27
|
||||
#define WEIGHT_SCALE 165 // //A通道是165,B通道是41
|
||||
#define HX711_GAIN 128
|
||||
#else
|
||||
// Moto-CAN //第二版本硬件参数---2号机使用
|
||||
#define MOTO_CAN_RX 27 // PCB画板需要，做了调整
|
||||
#define MOTO_CAN_TX 26 // PCB画板需要，做了调整
|
||||
#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41
|
||||
#define HX711_GAIN 32 // 减少零点漂移用B通道的感度
|
||||
// Moto-CAN //第二版本硬件参数---2号机使用
|
||||
#define MOTO_CAN_RX 27 //PCB画板需要，做了调整
|
||||
#define MOTO_CAN_TX 26 //PCB画板需要，做了调整
|
||||
#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41
|
||||
#define HX711_GAIN 32 //减少零点漂移用B通道的感度
|
||||
#endif
|
||||
|
||||
/// serial1
|
||||
///serial1
|
||||
#define SERIAL_REPORT_TX 5
|
||||
#define SERIAL_REPORT_RX 18
|
||||
/////
|
||||
// #define WEIGHT_SCALE 41 // //A通道是165,B通道是41
|
||||
#define TM_INSTORE_WEIGHT_DELAY 200 // 200 // 入仓动力延时关闭时间 ms
|
||||
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
|
||||
//#define WEIGHT_SCALE 41 // //A通道是165,B通道是41
|
||||
#define TM_INSTORE_WEIGHT_DELAY 200 //200 // 入仓动力延时关闭时间 ms
|
||||
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
|
||||
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
|
||||
|
||||
|
||||
enum HookStatus
|
||||
{
|
||||
HS_UnInit, // 还未初始化
|
||||
@ -66,9 +67,9 @@ enum Initstatus
|
||||
IS_CheckWeightZero, // 检查称重传感器是否归0
|
||||
IS_OK, // 4.所有初始化已经OK，可以正常操作
|
||||
IS_Error // 5.初始化遇到错误
|
||||
};
|
||||
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_STATUS = 42; // 发给飞控的状态
|
||||
const uint16_t MAV_CMD_FC_HOOK_RECOVERY = 43; // 飞控发的---收线
|
||||
const uint16_t MAV_CMD_FC_DESCENTSTART = 44; // 飞开始下降，需要调整相机向下
|
||||
} ;
|
||||
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_STATUS=42; //发给飞控的状态
|
||||
const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
|
||||
const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下
|
||||
|
||||
@ -804,5 +804,3 @@ void testbtn_click()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////MQTT_语音_MAVLINK 部分
|
||||
|
||||
12
src/moto.cpp
12
src/moto.cpp
@ -5,20 +5,20 @@
|
||||
uint8_t CaninBuffer[8]; // 接收指令缓冲区
|
||||
moto_measure_t moto_chassis;
|
||||
PID_TypeDef moto_pid;
|
||||
static const char *MOUDLENAME = "MOTO";
|
||||
static const char* MOUDLENAME = "MOTO";
|
||||
moto::moto()
|
||||
{
|
||||
_closed = true;
|
||||
}
|
||||
bool moto::init()
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "init moto");
|
||||
ESP_LOGD(MOUDLENAME,"init moto");
|
||||
pid_init();
|
||||
CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
|
||||
// start the CAN bus at 500 kbps
|
||||
if (!CAN.begin(1000E3))
|
||||
{
|
||||
ESP_LOGE(MOUDLENAME, "Starting CAN failed!");
|
||||
ESP_LOGE(MOUDLENAME,"Starting CAN failed!");
|
||||
return false;
|
||||
}
|
||||
CAN.onReceive(onReceive);
|
||||
@ -30,7 +30,7 @@ moto::~moto()
|
||||
|
||||
void moto::update()
|
||||
{
|
||||
// printf("u1:%d\n",_closed);
|
||||
//printf("u1:%d\n",_closed);
|
||||
if (!_closed)
|
||||
{
|
||||
unsigned long dt = millis() - moto_chassis.starttime; // 时间差
|
||||
@ -45,7 +45,7 @@ void moto::update()
|
||||
_msoftspeed = _start_speed + mspeed * _ds;
|
||||
}
|
||||
pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
|
||||
// printf("u2\n");
|
||||
// printf("u2\n");
|
||||
set_moto_current(moto_pid.output);
|
||||
// printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
|
||||
// moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
|
||||
@ -66,7 +66,7 @@ void moto::setspeedtarget(float new_target)
|
||||
_start_speed = moto_chassis.speed_rpm;
|
||||
_ds = moto_pid.target - _start_speed; // 速度差
|
||||
_closed = false;
|
||||
// printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
|
||||
// printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
|
||||
}
|
||||
|
||||
void moto::set_moto_current(int16_t iq1)
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
#include "config.h"
|
||||
|
||||
#define DEBUG_OUT false
|
||||
static const char *MOUDLENAME = "MOTO_C";
|
||||
static const char* MOUDLENAME = "MOTO_C";
|
||||
|
||||
Motocontrol::Motocontrol()
|
||||
{
|
||||
@ -23,7 +23,7 @@ Motocontrol::Motocontrol()
|
||||
_weightalign = false;
|
||||
_overweightcount = 0;
|
||||
_notweightcount = 0;
|
||||
_controlstatus.is_autogoodsdown = false;
|
||||
_controlstatus.is_autogoodsdown=false;
|
||||
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
|
||||
_unblocktimes = 0;
|
||||
}
|
||||
@ -51,53 +51,55 @@ void Motocontrol::setweight(int pullweight) // 设置重量
|
||||
_pullweight = pullweight;
|
||||
checkgoods();
|
||||
}
|
||||
String Motocontrol::gethooktatus_str(bool chstr)
|
||||
String Motocontrol::gethooktatus_str(bool chstr)
|
||||
{
|
||||
String hookstatusstr = "未知";
|
||||
String hookstatusstr_en = "unknown";
|
||||
switch (_hooksstatus)
|
||||
{
|
||||
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;
|
||||
hookstatusstr="未初始化";
|
||||
hookstatusstr_en="HS_UnInit";
|
||||
break;
|
||||
case HS_Down:
|
||||
hookstatusstr="放钩";
|
||||
hookstatusstr_en="HS_Down";
|
||||
break;
|
||||
case HS_DownSlow:
|
||||
hookstatusstr = "慢速放钩";
|
||||
hookstatusstr_en = "HS_DownSlow";
|
||||
break;
|
||||
hookstatusstr="慢速放钩";
|
||||
hookstatusstr_en="HS_DownSlow";
|
||||
break;
|
||||
case HS_WaitUnhook:
|
||||
hookstatusstr = "等待脱钩";
|
||||
hookstatusstr_en = "HS_WaitUnhook";
|
||||
break;
|
||||
hookstatusstr="等待脱钩";
|
||||
hookstatusstr_en="HS_WaitUnhook";
|
||||
break;
|
||||
case HS_Up:
|
||||
hookstatusstr = "回收";
|
||||
hookstatusstr_en = "HS_Up";
|
||||
break;
|
||||
hookstatusstr="回收";
|
||||
hookstatusstr_en="HS_Up";
|
||||
break;
|
||||
case HS_InStore:
|
||||
hookstatusstr = "入仓中";
|
||||
hookstatusstr_en = "HS_InStore";
|
||||
break;
|
||||
hookstatusstr="入仓中";
|
||||
hookstatusstr_en="HS_InStore";
|
||||
break;
|
||||
case HS_Locked:
|
||||
hookstatusstr = "已锁定";
|
||||
hookstatusstr_en = "HS_Locked";
|
||||
break;
|
||||
hookstatusstr="已锁定";
|
||||
hookstatusstr_en="HS_Locked";
|
||||
break;
|
||||
case HS_Stop:
|
||||
hookstatusstr = "停止";
|
||||
hookstatusstr_en = "HS_Stop";
|
||||
break;
|
||||
default:
|
||||
hookstatusstr = "未知";
|
||||
hookstatusstr_en = "HS_UnInit";
|
||||
break;
|
||||
}
|
||||
hookstatusstr="停止";
|
||||
hookstatusstr_en="HS_Stop";
|
||||
break;
|
||||
default:
|
||||
hookstatusstr="未知";
|
||||
hookstatusstr_en="HS_UnInit";
|
||||
break;
|
||||
}
|
||||
if (chstr)
|
||||
return hookstatusstr;
|
||||
else
|
||||
else
|
||||
return hookstatusstr_en;
|
||||
|
||||
|
||||
}
|
||||
|
||||
void Motocontrol::checkgoods() // 检测是否超重
|
||||
@ -139,13 +141,13 @@ void Motocontrol::checkgoods() // 检测是否超重
|
||||
}
|
||||
void Motocontrol::lockservo() // 锁定舵机
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "start_lockservo");
|
||||
ESP_LOGD(MOUDLENAME,"start_lockservo");
|
||||
_servotatus = SS_WaitMotoStop;
|
||||
_tm_servotatus = millis();
|
||||
}
|
||||
void Motocontrol::unlockservo() // 解锁舵机
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "unlockservo");
|
||||
ESP_LOGD(MOUDLENAME,"unlockservo");
|
||||
// 解锁操作
|
||||
_lockservo->write(SERVO_UNLOCKPOS);
|
||||
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
|
||||
@ -155,25 +157,25 @@ void Motocontrol::unlockservo() // 解锁舵机
|
||||
}
|
||||
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.setspeedtarget(0.0f);
|
||||
_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;
|
||||
sethooksstatus(HS_Stop);
|
||||
}
|
||||
lockservo();
|
||||
}
|
||||
void Motocontrol::stopautodown()
|
||||
void Motocontrol::stopautodown()
|
||||
{
|
||||
_controlstatus.is_autogoodsdown = false;
|
||||
}
|
||||
void Motocontrol::sethooksstatus(HookStatus hookstatus)
|
||||
{
|
||||
_hooksstatus = hookstatus;
|
||||
ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false));
|
||||
_hooksstatus=hookstatus;
|
||||
ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false));
|
||||
}
|
||||
void Motocontrol::setlocked(bool locked)
|
||||
{
|
||||
@ -197,6 +199,7 @@ int16_t Motocontrol::getlength() // 得到长度
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 重量传感器已经校准
|
||||
void Motocontrol::weightalign(bool weightalign)
|
||||
{
|
||||
@ -217,14 +220,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
// 如果没有货物--开始回收
|
||||
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);
|
||||
_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);
|
||||
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);
|
||||
@ -236,7 +239,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
{
|
||||
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);
|
||||
_tm_waitunhook = millis();
|
||||
}
|
||||
@ -247,7 +250,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
{
|
||||
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);
|
||||
set_hook_speed(-SPEED_HOOK_UP);
|
||||
sethooksstatus(HS_Up);
|
||||
@ -313,14 +316,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
|
||||
if (!checkservoblock() || (_unblocktimes > 2))
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "Close moto");
|
||||
ESP_LOGD(MOUDLENAME,"Close moto");
|
||||
_moto_dji.close();
|
||||
_servotatus = SS_ServoLocked;
|
||||
}
|
||||
else
|
||||
// 堵转
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "SS servoblock unlock servo and turn moto");
|
||||
ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto");
|
||||
_tm_servotatus = millis();
|
||||
_servotatus = SS_WaitUnBlock;
|
||||
// 写一个不会堵转的位置
|
||||
@ -336,7 +339,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
if ((millis() - _tm_servotatus) > TM_UNBLOCK)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "SS lock servo");
|
||||
ESP_LOGD(MOUDLENAME,"SS lock servo");
|
||||
// 继续锁定等待舵机检测
|
||||
_lockservo->write(SERVO_LOCKPOS);
|
||||
_servotatus = SS_WaitServo;
|
||||
@ -370,12 +373,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
_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);
|
||||
ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
|
||||
//已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
|
||||
if (_hooksstatus != HS_InStore)
|
||||
_controlstatus.is_instore = false;
|
||||
}
|
||||
@ -386,7 +389,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
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); // 缓慢停止
|
||||
}
|
||||
}
|
||||
@ -394,7 +397,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
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); // 缓慢停止
|
||||
}
|
||||
}
|
||||
@ -429,8 +432,7 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
|
||||
|
||||
void Motocontrol::moto_goodsdownresume()
|
||||
{
|
||||
if (!_controlstatus.is_autogoodsdown)
|
||||
return;
|
||||
if (!_controlstatus.is_autogoodsdown) return;
|
||||
if (_hooksstatus == HS_Stop)
|
||||
{
|
||||
_runspeed = _goods_speed;
|
||||
@ -447,29 +449,29 @@ void Motocontrol::moto_goodsdownresume()
|
||||
// _moto_dji.setspeedtarget(_goods_speed);
|
||||
}
|
||||
}
|
||||
// 长度cm
|
||||
//长度cm
|
||||
void Motocontrol::moto_goodsdown(float length)
|
||||
{
|
||||
if (length < 0.0)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "length<0 fault");
|
||||
ESP_LOGD(MOUDLENAME,"length<0 fault");
|
||||
return;
|
||||
}
|
||||
// 没设置零点
|
||||
if (!_controlstatus.is_setzero)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "not lengthzero fault");
|
||||
ESP_LOGD(MOUDLENAME,"not lengthzero fault");
|
||||
return;
|
||||
}
|
||||
if (!_weightalign)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "weight not align fault");
|
||||
ESP_LOGD(MOUDLENAME,"weight not align fault");
|
||||
return;
|
||||
}
|
||||
// 没挂东西
|
||||
if (!_controlstatus.is_havegoods)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight);
|
||||
ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -485,12 +487,12 @@ void Motocontrol::moto_goodsdown(float length)
|
||||
}
|
||||
else
|
||||
{
|
||||
float tarleng = length;
|
||||
if (length > HOOK_SLOWDOWN_LENGTH)
|
||||
tarleng -= HOOK_SLOWDOWN_LENGTH;
|
||||
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);
|
||||
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;
|
||||
}
|
||||
@ -510,25 +512,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
// 运动中暂时不管
|
||||
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;
|
||||
}
|
||||
// 没设置速度不转
|
||||
if (_controlstatus.speed_targe == 0)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "not set speed_targe");
|
||||
ESP_LOGD(MOUDLENAME,"not set speed_targe");
|
||||
return;
|
||||
}
|
||||
if (updown == MS_Up)
|
||||
{
|
||||
if (_controlstatus.is_toplocked)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "is_toplocked return");
|
||||
ESP_LOGD(MOUDLENAME,"is_toplocked return");
|
||||
return;
|
||||
}
|
||||
if (_controlstatus.is_overweight)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight);
|
||||
ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight);
|
||||
return;
|
||||
}
|
||||
sethooksstatus(HS_Up);
|
||||
@ -565,7 +567,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
runspeed = -_controlstatus.speed_targe;
|
||||
}
|
||||
// 开始转
|
||||
// ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
|
||||
// ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
|
||||
_runspeed = runspeed;
|
||||
// _moto_dji.setspeedtarget(runspeed);
|
||||
}
|
||||
@ -9,7 +9,7 @@
|
||||
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
|
||||
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
|
||||
|
||||
#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
#define INSTORE_LENGTH_MIN_GOODS 10.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
|
||||
#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
|
||||
@ -20,8 +20,8 @@
|
||||
#define SPEED_BTN_MID 100 // 按键中等收放线速度
|
||||
#define SPEED_BTN_FAST 200 // 按键最快收放线速度
|
||||
#define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
|
||||
#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
|
||||
#define SPEED_HOOK_UP SPEED_MAX // 钩子回收速度
|
||||
#define SPEED_UNBLOCK 0.1 // 舵机堵转需要缓慢转的速度
|
||||
|
||||
@ -29,8 +29,8 @@
|
||||
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
|
||||
#define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
|
||||
|
||||
#define SERVO_LOCKPOS 1000 // 1920 // 舵机锁定位置
|
||||
#define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置
|
||||
#define SERVO_LOCKPOS 1000 //1920 // 舵机锁定位置
|
||||
#define SERVO_UNLOCKPOS 1120 //1800 // 舵机解锁位置
|
||||
#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
|
||||
|
||||
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
|
||||
@ -55,7 +55,7 @@ typedef struct
|
||||
bool is_toplocked; // 已到顶锁住
|
||||
bool is_overweight; // 是否超重
|
||||
bool is_havegoods; // 是否有货物
|
||||
bool is_autogoodsdown; // 正在自动放线中
|
||||
bool is_autogoodsdown; //正在自动放线中
|
||||
float zero_cnt; // 0长度位置
|
||||
float speed_targe; // 当前目标速度
|
||||
MotoStatus motostatus; // 电机运行状态
|
||||
@ -111,13 +111,12 @@ private:
|
||||
void unlockservo();
|
||||
void set_hook_speed(float hooksspeed);
|
||||
void sethooksstatus(HookStatus hookstatus);
|
||||
|
||||
public:
|
||||
Motocontrol(); // 构造函数
|
||||
~Motocontrol(); // 析构函数
|
||||
void setspeed(float motospeed, float acctime = -1); // 设置速度
|
||||
void stoprun(float acctime = 0); // 停止运行
|
||||
void stopautodown(); // 停止自动下放模式
|
||||
void stoprun(float acctime = 0); // 停止运行
|
||||
void stopautodown(); //停止自动下放模式
|
||||
void setzero(); // 设置0长度位置
|
||||
int16_t getlength(); // 得到长度
|
||||
void update(); // 更新
|
||||
@ -131,7 +130,7 @@ public:
|
||||
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
|
||||
void moto_goodsdownresume(); // 恢复自动放线
|
||||
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
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user