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

加入飞控中途暂停并回收功能

Browse Source 
调整入仓长度，到地速度以及总体速度等参数
This commit is contained in:
pxzleo 2023-07-01 21:14:09 +08:00
parent 292fd89305
commit b60f2a3cc1
2 changed files with 24 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main.cpp
View File

@ -85,6 +85,8 @@ unsigned long _tm_waitinit;
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; //飞控发的---收线
///////////////////////
/////////////////////////////////MQTT_语音_MAVLINK 部分
@ -698,7 +700,15 @@ void Hook_resume()
}else
Serial.println("resume fault, not HS_Stop ");
}
void Hook_recovery()
{
Serial.println("Hook_recovery");
if (motocontrol.gethooktatus()!=HS_Locked)
{
motocontrol.stoprun();
up_action(SPEED_HOOK_UP);
}
}
// 测试按钮
void testbtn_click()
@ -1259,6 +1269,13 @@ void mavlink_receiveCallback(uint8_t c) {
Hook_stop();
break;
}
//暂停
case MAV_CMD_FC_HOOK_RECOVERY:
{
printf("MAV_CMD_FC_HOOK_RECOVERY \n");
Hook_recovery();
break;
}
}
}

12
src/motocontrol.h
View File

@ -9,20 +9,20 @@
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
#define INSTORE_LENGTH_MIN_NONE 10.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_MIN_GOODS 8.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最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
#define SPEED_MAX 350 // 最快收放线速度--任何时候不应该超过这个速度def:350
#define SPEED_MAX 400 // 最快收放线速度--任何时候不应该超过这个速度def:350
#define SPEED_INSTORE 5 // 入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20 // 按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#define SPEED_BTN_MID 100 // 按键中等收放线速度
#define SPEED_BTN_FAST 200 // 按键最快收放线速度
#define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20 // 货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP SPEED_MAX // 空钩上升速度
#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP SPEED_MAX // 钩子回收速度
#define SPEED_UNBLOCK 0.1 // 舵机堵转需要缓慢转的速度
#define TM_UNBLOCK 100 // 舵机堵转需要缓慢转等待的时间ms