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

重构部分放线收线功能用于外部控制

Browse Source 
修改重量校准值
锁定延时加入重量判断，空的时候不延时，防止空载线压得太紧

控制功能：
飞机按键：
	动作	单击	双击	长按
	收线按钮	慢速收线	快速收线	急速收线
	放线按钮	慢速放线/停止	快速放线	急速放线
	1.收线或放线中任意按钮停止收放线
	2.自动放线中，任意按钮暂停自动放线，再单击(双击)放线就是继续自动放线，单击收线则终止自动放线并慢速收线------(只用于测试)
	3.收放线按钮同时按是对频

pad按键--(仅在自动放线中有效)
	收线按钮	终止自动放线并回收
	暂停	暂停自动放线
	继续	继续自动放线
This commit is contained in:
pxzleo 2023-06-27 02:02:21 +08:00
parent 3200e60129
commit 7da06e6d06
5 changed files with 120 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/FoodDeliveryBase.cpp
View File

@ -129,7 +129,7 @@ void FoodCube::connectWifi() {
macAdd.replace(":", ""); //板子的物理地址 并且去除冒号
log("macAdd: ");
logln(macAdd);
playText("网络连接成功");
playText("[v1]网络连接成功");
delay(500);
}
@ -151,7 +151,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题
subscribeTopic(topicSub[i], 1);
}
playText("指令服务器已连接");
playText("[v1]指令服务器已连接");
} else {
//失败返回状态码
log("MQTT Server Connect Failed. Client State:");

5
src/config.h
View File

@ -24,8 +24,9 @@
#define SERIAL_REPORT_TX 5
#define SERIAL_REPORT_RX 18
/////
#define WEIGHT_SCALE 276 // 这是缩放值，根据砝码实测516.f
#define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms
#define WEIGHT_SCALE 165 // 276 //这是缩放值，根据砝码实测516.f
#define TM_INSTORE_DELAY 200 //200 // 入仓动力延时关闭时间 ms
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms

158
src/main.cpp
View File

@ -57,6 +57,7 @@ void Task1(void *pvParameters);
// void Task1code( void *pvParameters );
void showledidel();
int pullweight = 0; //检测重量-克
int pullweightoktimes=0; //校准成功次数
int8_t btn_pressatonce=0; //是否同时按下
unsigned long _tm_bengstop;
bool _bengstop = false;
@ -186,7 +187,7 @@ void setup()
/////////////////////////////////MQTT_语音_MAVLINK 部分
/*初始化*/
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射
fc.playText("正在连接网络");
fc.playText("[v1]正在连接网络");
fc.connectWifi(); //连接wifi
// fc.playText("正在连接服务器");
fc.connectMqtt(topicSub, topicSubCount); //连接mqtt
@ -207,7 +208,7 @@ void setup()
// //slowup();
// }
fc.playText("启动完成");
fc.playText("[v1]启动完成");
Serial.println("PullupDevice is ready!");
}
void slowup()
@ -245,6 +246,7 @@ void checkinited()
case Initstatus::IS_Start:
{
//一开始没有锁定状态
if (motocontrol.gethooktatus() != HS_Locked)
{
// 开始自动慢速上升，直到顶部按钮按下
@ -253,7 +255,7 @@ void checkinited()
motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_Locked;
}
else
else //开机就是锁定状态--开始校准称重传感器
{
_needweightalign = true;
// scale.tare(10);
@ -279,17 +281,22 @@ void checkinited()
case Initstatus::IS_CheckWeightZero:
{
// 重量校准成功
if (pullweight < 10)
if ((pullweight < 10)&&((pullweight >-10)))
{
pullweightoktimes++;
if (pullweightoktimes>20)
{
motocontrol.weightalign(true);
initstatus = IS_OK;
printf("WeightAlign ok: %d \n", pullweight);
}else _needweightalign = true;
}
else
{
// 没成功继续校准
printf("pullweight fault: %d \n", pullweight);
_needweightalign = true;
pullweightoktimes=0;
// scale.tare(10);
}
break;
@ -356,11 +363,22 @@ void loop()
showinfo(); // 显示一些调试用信息
// 到顶后延迟关闭动力电和舵机
if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY))
if (_bengstop)
{
if ((initstatus==IS_OK)&&(pullweight>TM_INSTORE_DELAY_WEIGHT) )
{
if ((millis() - _tm_bengstop) > TM_INSTORE_DELAY)
{
_bengstop = false;
motocontrol.setlocked(true);
}
}
else
{
_bengstop = false;
motocontrol.setlocked(true);
}
}
// 检测执行初始化工作
checkinited();
@ -390,6 +408,7 @@ void Task1(void *pvParameters)
scale.tare();
}
pullweight = get_pullweight();
//显示重量
//printf("pullweight: %d \n", pullweight);
vTaskDelay(10);
@ -411,7 +430,7 @@ int get_pullweight()
else
NewValue = 0;
Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
Value = constrain(Value, 0.0, 6000.0); // 限制到0-6公斤
Value = constrain(Value, 0.0, 10000); // 限制到0-10公斤
return round(Value);
}
// 提示灯光控制
@ -490,37 +509,49 @@ void ctbtn_pressend()
_bengstop = false;
}
void down_action(float motospeed)
{
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stoprun();
}
else
{
//如果在自动放线状态，只是恢复自动放线
if (motocontrol.getcontrolstatus().is_autogoodsdown)
{
motocontrol.moto_goodsdownresume();
}else
{
motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Down);
fc.playText("[v1]放线");
}
}
}
void up_action(float motospeed)
{
if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
{
motocontrol.stopautodown(); //终止自动放线
motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Up);
fc.playText("[v1]收线");
}else
motocontrol.stoprun();
}
// 放线按钮--单击
void downbtn_click()
{
Serial.println("downbtn_click");
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stop();
}
else
{
motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Down);
fc.playText("放线");
}
down_action(SPEED_BTN_SLOW);
}
// 放线按钮--双击
void downbtn_dbclick()
{
Serial.println("downbtn_dbclick");
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stop();
}
else
{
motocontrol.setspeed(SPEED_BTN_MID);
motocontrol.moto_run(MotoStatus::MS_Down);
}
down_action(SPEED_BTN_MID);
}
// 放线按钮--长按
void downbtn_pressstart()
@ -534,17 +565,7 @@ void downbtn_pressstart()
btn_presssatonce();
return;
}
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stop();
}
else
{
motocontrol.setspeed(SPEED_BTN_FAST);
motocontrol.moto_run(MotoStatus::MS_Down);
}
down_action(SPEED_BTN_FAST);
}
// 放线按钮--长按抬起
void downbtn_pressend()
@ -552,46 +573,30 @@ void downbtn_pressend()
Serial.println("downbtn_pressend");
btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0;
motocontrol.stop();
//不是恢复自动放线抬起按键就停止
if (!motocontrol.getcontrolstatus().is_autogoodsdown)
motocontrol.stoprun();
}
// 收线按钮-单击
void upbtn_click()
{
Serial.println("upbtn_click");
up_action(SPEED_BTN_SLOW);
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stop();
}
else
{
fc.playText("收线");
motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up);
}
}
// 收线按钮-双击
void upbtn_dbclick()
{
Serial.println("upbtn_dbclick");
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stop();
}
else
{
motocontrol.setspeed(SPEED_BTN_MID);
motocontrol.moto_run(MotoStatus::MS_Up);
}
up_action(SPEED_BTN_MID);
}
// 两个按钮同时按下
void btn_presssatonce()
{
Serial.println("btn_presssatonce");
led_show(255,255, 255); // 高亮一下
fc.playText("发送对频信息");
fc.playText("[v1]发送对频信息");
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
}
// 收线按钮-长按
@ -606,16 +611,7 @@ void upbtn_pressstart()
btn_presssatonce();
return;
}
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
motocontrol.stop();
}
else
{
motocontrol.setspeed(SPEED_BTN_FAST);
motocontrol.moto_run(MotoStatus::MS_Up);
}
up_action(SPEED_BTN_FAST);
}
// 收线按钮-长按抬起
void upbtn_pressend()
@ -624,7 +620,7 @@ void upbtn_pressend()
btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0;
motocontrol.stop();
motocontrol.stoprun();
}
//自动下放
@ -640,7 +636,7 @@ void Hook_autodown(float length_cm)
void Hook_stop()
{
Serial.println("Hook_stop");
motocontrol.stop();
motocontrol.stoprun();
}
void Hook_resume()
{
@ -675,7 +671,7 @@ void testbtn_click()
default:
{
Serial.println("stop");
motocontrol.stop();
motocontrol.stoprun();
}
}
}
@ -799,11 +795,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
case 0: //收
/* code */
printf("Mqtt_HOOK_UP \n");
if (motocontrol.gethooktatus()==HS_Stop)
{
motocontrol.setspeed(SPEED_BTN_FAST);
motocontrol.moto_run(MotoStatus::MS_Up);
}
if (motocontrol.getcontrolstatus().is_autogoodsdown)
up_action(SPEED_BTN_FAST);
break;
case 1: //放
/* code */
@ -811,12 +804,13 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
case 2: //暂停
/* code */
printf("Mqtt_HOOK_PAUSE \n");
if (motocontrol.getcontrolstatus().is_autogoodsdown)
Hook_stop();
break;
case 3: //继续
/* code */
printf("Mqtt_HOOK_resume \n");
if (motocontrol.gethooktatus()==HS_Stop)
if (motocontrol.getcontrolstatus().is_autogoodsdown)
Hook_resume();
break;
}
@ -949,7 +943,7 @@ void mavlink_receiveCallback(uint8_t c) {
if (!curr_armed)
{
printf("armed\n");
fc.playText("已解锁");
fc.playText("[v1]已解锁");
}
curr_armed=true;
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
@ -957,7 +951,7 @@ void mavlink_receiveCallback(uint8_t c) {
if (curr_armed)
{
printf("disarm\n");
fc.playText("已加锁");
fc.playText("[v1]已加锁");
}
curr_armed=false; //心跳里面 判断没有解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁

40
src/motocontrol.cpp
View File

@ -21,7 +21,7 @@ Motocontrol::Motocontrol()
_weightalign = false;
_overweightcount = 0;
_notweightcount = 0;
_autogoodsdown = false;
_controlstatus.is_autogoodsdown=false;
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
_unblocktimes = 0;
}
@ -141,27 +141,33 @@ void Motocontrol::unlockservo() // 解锁舵机
_tm_servotatus = millis();
_unblocktimes = 0;
}
void Motocontrol::stop(float acctime) // 停止
void Motocontrol::stoprun(float acctime) // 停止
{
printf("stop \n");
_moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop;
if ((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop))
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) )
{
_hooksstatus_prv = _hooksstatus;
_hooksstatus = HS_Stop;
printf("HS_Stop \n");
}
lockservo();
}
void Motocontrol::stopautodown()
{
_controlstatus.is_autogoodsdown = false;
}
void Motocontrol::setlocked(bool locked)
{
if (locked)
{
stop();
stoprun();
setzero();
_hooksstatus = HS_Locked;
_autogoodsdown = false;
printf("HS_Locked \n");
_controlstatus.is_autogoodsdown = false;
}
_controlstatus.is_toplocked = locked;
}
@ -190,7 +196,7 @@ void Motocontrol::weightalign(bool weightalign)
void Motocontrol::checkhookstatus() // 检查挂钩状态
{
// 需要在自动放货物中
if (!_autogoodsdown)
if (!_controlstatus.is_autogoodsdown)
return;
moto_measure_t mf = _moto_dji.getmotoinfo();
_hook_currlen = _curr_length;
@ -202,12 +208,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 如果没有货物--开始回收
if (!_controlstatus.is_havegoods)
{
printf("HS_Down not havegoods wait unhook \n");
printf("HS_Down check not goods wait %d ms for unhook \n",HOOK_UNHOOK_TIME);
goodsstartup();
break;
}
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
{
printf("HS_Down target is ok curr: %.2f slow \n",mf.output_round_cnt);
_hooksstatus = HS_DownSlow;
_moto_dji.settime_acc(500);
set_hook_speed(SPEED_HOOK_SLOW);
@ -336,7 +344,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
_curr_length = abs(mf.output_round_cnt - _controlstatus.zero_cnt) * WHEEL_PERIMETER;
// 不能超ROPE_MAXLENGTH
if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
stop();
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;
@ -365,7 +373,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if (mf.output_round_cnt > _target_cnt)
{
printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop(1000); // 缓慢停止
stoprun(1000); // 缓慢停止
}
}
else if (_controlstatus.motostatus == MS_Up)
@ -373,7 +381,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if (mf.output_round_cnt < _target_cnt)
{
printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop(1000); // 缓慢停止
stoprun(1000); // 缓慢停止
}
}
}
@ -412,7 +420,7 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdownresume()
{
if (!_autogoodsdown) return;
if (!_controlstatus.is_autogoodsdown) return;
if (_hooksstatus == HS_Stop)
{
_runspeed = _goods_speed;
@ -456,7 +464,7 @@ void Motocontrol::moto_goodsdown(float length)
}
// 开始自动放货物状态
_autogoodsdown = true;
_controlstatus.is_autogoodsdown = true;
// 长度=0，直接中慢速下降，
if (length == 0.0)
@ -467,8 +475,12 @@ void Motocontrol::moto_goodsdown(float length)
}
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 + length / WHEEL_PERIMETER;
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
printf("start down %.2f cm,tar:%.2f \n",tarleng,_goods_down_target_cnt);
setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST;
}
@ -481,7 +493,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 传入要stop，直接停
if (updown == MS_Stop)
{
stop();
stoprun();
return;
}

9
src/motocontrol.h
View File

@ -14,7 +14,7 @@
#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
#define SPEED_MAX 350 // 最快收放线速度--任何时候不应该超过这个速度
#define SPEED_MAX 350 // 最快收放线速度--任何时候不应该超过这个速度def:350
#define SPEED_INSTORE 5 // 入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20 // 按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#define SPEED_BTN_MID 100 // 按键中等收放线速度
@ -37,7 +37,7 @@
#define HOOK_WEIHT_MAX 6000 // 最大货物重量 大于这个认为超重不工作 (克)
#define HOOK_SLOWDOWN_LENGTH 10 // 下放物体时快到目标长度时慢速长度 (cm)
#define HOOK_SLOWDOWN_LENGTH 50 // 下放物体时快到目标长度时慢速长度 (cm)
#define HOOK_UNHOOK_TIME 1500 // 等待脱钩时间ms
// 挂钩状态
@ -55,6 +55,7 @@ typedef struct
bool is_toplocked; // 已到顶锁住
bool is_overweight; // 是否超重
bool is_havegoods; // 是否有货物
bool is_autogoodsdown; //正在自动放线中
float zero_cnt; // 0长度位置
float speed_targe; // 当前目标速度
MotoStatus motostatus; // 电机运行状态
@ -95,7 +96,6 @@ private:
int _hook_currlen;
float _curr_length;
bool _weightalign;
bool _autogoodsdown;
uint8_t _overweightcount;
uint8_t _notweightcount;
@ -116,7 +116,8 @@ public:
Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数
void setspeed(float motospeed, float acctime = -1); // 设置速度
void stop(float acctime = 0); // 停止
void stoprun(float acctime = 0); // 停止运行
void stopautodown(); //停止自动下放模式
void setzero(); // 设置0长度位置
int16_t getlength(); // 得到长度
void update(); // 更新