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(":", ""); //板子的物理地址 并且去除冒号 macAdd.replace(":", ""); //板子的物理地址 并且去除冒号
log("macAdd: "); log("macAdd: ");
logln(macAdd); logln(macAdd);
playText("网络连接成功"); playText("[v1]网络连接成功");
delay(500); delay(500);
} }
@ -151,7 +151,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题 for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题
subscribeTopic(topicSub[i], 1); subscribeTopic(topicSub[i], 1);
} }
playText("指令服务器已连接"); playText("[v1]指令服务器已连接");
} else { } else {
//失败返回状态码 //失败返回状态码
log("MQTT Server Connect Failed. Client State:"); 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_TX 5
#define SERIAL_REPORT_RX 18 #define SERIAL_REPORT_RX 18
///// /////
#define WEIGHT_SCALE 276 // 这是缩放值，根据砝码实测516.f #define WEIGHT_SCALE 165 // 276 //这是缩放值，根据砝码实测516.f
#define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms #define TM_INSTORE_DELAY 200 //200 // 入仓动力延时关闭时间 ms
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms

160
src/main.cpp
View File

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

40
src/motocontrol.cpp
View File

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

9
src/motocontrol.h
View File

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