diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 0597a77..05869d1 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -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:"); diff --git a/src/config.h b/src/config.h index c9c8b4f..6d00351 100644 --- a/src/config.h +++ b/src/config.h @@ -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 diff --git a/src/main.cpp b/src/main.cpp index 900967b..6b21339 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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))) { - motocontrol.weightalign(true); - initstatus = IS_OK; - printf("WeightAlign ok: %d \n", pullweight); + 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,10 +363,21 @@ void loop() showinfo(); // 显示一些调试用信息 // 到顶后延迟关闭动力电和舵机 - if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY)) + if (_bengstop) { - _bengstop = false; - motocontrol.setlocked(true); + 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); + } } // 检测执行初始化工作 @@ -390,7 +408,8 @@ void Task1(void *pvParameters) scale.tare(); } pullweight = get_pullweight(); - // printf("pullweight: %d \n", 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"); - - if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) - { - motocontrol.stop(); - } - else - { - fc.playText("收线"); - motocontrol.setspeed(SPEED_BTN_SLOW); - motocontrol.moto_run(MotoStatus::MS_Up); - } + up_action(SPEED_BTN_SLOW); + } // 收线按钮-双击 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"); - Hook_stop(); + 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 代表未解锁 diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index b94162f..0e5f984 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -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; } diff --git a/src/motocontrol.h b/src/motocontrol.h index e704d10..5d6dd4d 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -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(); // 更新