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

修改自动放物体的bug，整理代码

Browse Source
This commit is contained in:
pxzleo 2023-04-25 22:13:04 +08:00
parent 50e470f135
commit 2f0e904107
4 changed files with 263 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/config.h
View File

@ -10,22 +10,25 @@
#define BTN_DOWN 22 // 放线开关
#define BTN_CT 21 // 到顶检测开关
#define BTN_TEST 18 // 测试开关
// LED
#define LED_TIP 19 // 指示灯 接线：LED_TIP---3.3V
// 称重传感器- HX711
#define LOADCELL_DOUT_PIN 2
#define LOADCELL_SCK_PIN 4
#define LOADCELL_DOUT_PIN 16
#define LOADCELL_SCK_PIN 17
///////////////////////////////////////////
#define SPEED_UP_SLOW 4 //rmp 或者 弧度/秒
#define SERVO_PIN 14 //锁定舵机PWM控制脚
////LED
#define LED_DATA_PIN 25
/////
#define WEIGHT_SCALE 276 //这是缩放值，根据砝码实测516.f
#define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms
enum HookStatus
{
HS_UnInit, //还未初始化
HS_Down, // 货物下放中
HS_DownSlow, //货物慢速下放中
HS_DownSlow, //货物慢速下放中 --接近地面慢速，不受设置速度影响
HS_WaitUnhook, //等待脱钩
HS_Up, // 回收中
HS_InStore, // 入仓中
HS_InStore, // 入仓中 --慢速，不受设置速度影响
HS_Locked, //已到顶部锁定
HS_Stop //已停止
};

202
src/main.cpp
View File

@ -9,7 +9,7 @@
#include "moto.h"
#include <FastLED.h>
#include <ESP32Servo.h>
#define TM_INSTORE_DELAY 100 // 入仓延时关闭时间 ms
// 角度传感器
// 收放线电机控制
@ -26,7 +26,7 @@ HX711 scale;
Serialcommand sercomm;
Servo myservo;
#define NUM_LEDS 1
#define DATA_PIN 25
CRGB leds[NUM_LEDS];
Motocontrol motocontrol;
@ -58,20 +58,22 @@ void showledidel();
int pullweight = 0;
unsigned long _tm_bengstop;
bool _bengstop = false;
bool _needweightalign = false;
// 需要做的初始化工作
enum Initstatus
{
IS_Start, // 0. 刚上电
IS_Wait_LengthZero,
IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线
IS_Start, // 0. 刚上电
IS_Wait_Locked,
IS_LengthZero, // 1.已确定零点
IS_Wait_WeightZero,
IS_WeightZero, // 2.已确定称重传感器0点
IS_InStoreLocked, // 3. 挂钩入仓顶锁定
IS_OK, // 4.所有初始化已经OK，可以正常操作
IS_Error // 5.初始化遇到错误
IS_WeightZero, // 2.已确定称重传感器0点
IS_InStoreLocked, // 3. 挂钩入仓顶锁定
IS_CheckWeightZero, // 检查称重传感器是否归0
IS_OK, // 4.所有初始化已经OK，可以正常操作
IS_Error // 5.初始化遇到错误
} initstatus;
unsigned long _tm_waitinit;
void setup()
{
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
@ -107,13 +109,15 @@ void setup()
myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object
// 初始化RGB LED 显示黄色灯表示需要注意 LED
FastLED.addLeds<WS2812, DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
if (!motocontrol.init(&myservo)) // 初始化电机控制
printf("motocontrol init fault\n");
// 发送状态任务开启--500ms一次
tksendinfo.start();
initstatus = IS_Start;
initstatus = IS_WaitStart;
_tm_waitinit = millis();
_needweightalign = true;
// if (motocontrol.getstatus()==MS_Stop)
// {
// //slowup();
@ -129,10 +133,17 @@ void slowup()
}
void showinfo()
{
// moto_measure_t mf=motocontrol.getmotoinfo() ;
// printf("total_cnt:%.3f;speed:%.2f,curr:%.2fA \n ", mf.output_round_cnt, mf.output_speed_rpm,(float)mf.real_current/1000.0f);
//if (pullweight > 10)
// printf("PullWeight:%d\n", pullweight);
// moto_measure_t mf=motocontrol.getmotoinfo() ;
// printf("total_cnt:%.3f;speed:%.2f,curr:%.2fA \n ", mf.output_round_cnt, mf.output_speed_rpm,(float)mf.real_current/1000.0f);
// if (pullweight > 10)
// printf("PullWeight:%d\n", pullweight);
// HookStatus hs=motocontrol.gethooktatus() ;
//control_status_t cs=motocontrol.getcontrolstatus() ;
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
}
// 初始化过程--
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
@ -141,63 +152,103 @@ void checkinited()
{
switch (initstatus)
{
case Initstatus::IS_Start:
case Initstatus::IS_WaitStart:
{
// 开始自动慢速上升，直到顶部按钮按下
printf("IS_Start: startup wait locked\n");
motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_LengthZero;
if ((millis() - _tm_waitinit) > 500)
initstatus = IS_Start;
break;
}
case Initstatus::IS_Wait_LengthZero:
case Initstatus::IS_Start:
{
if (motocontrol.gethooktatus() != HS_Locked)
{
// 开始自动慢速上升，直到顶部按钮按下
printf("IS_Start: startup wait locked\n");
motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_Locked;
}
else
{
_needweightalign = true;
// scale.tare(10);
// motocontrol.weightalign(true);
initstatus = IS_CheckWeightZero;
}
break;
}
case Initstatus::IS_Wait_Locked:
{
if (motocontrol.gethooktatus() == HS_Locked)
{
printf("IS_Wait_LengthZero: is locked\n");
initstatus = IS_LengthZero;
_needweightalign = true;
// scale.tare(10);
// motocontrol.weightalign(true);
initstatus = IS_CheckWeightZero;
// initstatus = IS_LengthZero;
}
break;
}
case Initstatus::IS_LengthZero:
case Initstatus::IS_CheckWeightZero:
{
// 开始自动慢速下降2cm
printf("IS_LengthZero: Down 2cm \n");
motocontrol.setspeed(SPEED_INSTORE);
motocontrol.moto_run(MotoStatus::MS_Down, 10);
initstatus = IS_Wait_WeightZero;
break;
}
case Initstatus::IS_Wait_WeightZero:
{
if (motocontrol.gethooktatus() == HS_Stop)
// 重量校准成功
if (pullweight < 10)
{
printf("IS_Wait_WeightZero: Down Stop start tare and up \n");
scale.tare();
motocontrol.weightalign(true);
motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_WeightZero;
initstatus = IS_OK;
}
break;
}
case Initstatus::IS_WeightZero:
{
if (motocontrol.gethooktatus() == HS_Locked)
else
{
printf("IS_WeightZero: Locked \n");
initstatus = IS_InStoreLocked;
// 没成功继续校准
printf("pullweight fault: %d \n", pullweight);
_needweightalign = true;
// scale.tare(10);
}
break;
}
case Initstatus::IS_InStoreLocked:
{
printf("IS_InStoreLocked: IS_OK \n");
initstatus = IS_OK;
break;
}
/*
case Initstatus::IS_LengthZero:
{
// 开始自动慢速下降2cm
printf("IS_LengthZero: Down 2cm \n");
motocontrol.setspeed(SPEED_INSTORE);
motocontrol.moto_run(MotoStatus::MS_Down, 10);
initstatus = IS_Wait_WeightZero;
break;
}
case Initstatus::IS_Wait_WeightZero:
{
if (motocontrol.gethooktatus() == HS_Stop)
{
printf("IS_Wait_WeightZero: Down Stop start tare and up \n");
scale.tare();
motocontrol.weightalign(true);
motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_WeightZero;
}
break;
}
case Initstatus::IS_WeightZero:
{
if (motocontrol.gethooktatus() == HS_Locked)
{
printf("IS_WeightZero: Locked \n");
initstatus = IS_InStoreLocked;
}
break;
}
case Initstatus::IS_InStoreLocked:
{
printf("IS_InStoreLocked: IS_OK \n");
initstatus = IS_OK;
break;
}
*/
}
}
// 在核心1上执行，重要的延迟低的
@ -228,12 +279,17 @@ void Task1(void *pvParameters)
{
// 初始化称重传感器
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
scale.set_scale(963.f); // 这是缩放值，根据砝码实测516.f
scale.tare(); // 重置为0
scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
scale.tare(); // 重置为0
// 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
while (1)
{
if (_needweightalign)
{
_needweightalign = false;
scale.tare();
}
pullweight = get_pullweight();
vTaskDelay(10);
}
@ -266,21 +322,26 @@ void led_show(uint8_t cr, uint8_t cg, uint8_t cb)
// 显示颜色
void showledidel()
{
// 超重--红色
if (motocontrol.getcontrolstatus().is_overweight)
{
led_show(255, 0, 0); // 红色
return;
}
switch (motocontrol.getcontrolstatus().motostatus)
{
case MotoStatus::MS_Down:
{
led_show(0, 0, 255); // blue
led_show(0, 0, 255); // 蓝色
break;
}
case MotoStatus::MS_Up:
{
if (motocontrol.getcontrolstatus().is_instore)
led_show(255, 0, 0); // red
led_show(255, 0, 0); // 红
else
led_show(200, 0, 50); // red
led_show(200, 0, 50); // 粉红
break;
}
@ -288,22 +349,27 @@ void showledidel()
{
if (motocontrol.getcontrolstatus().is_toplocked)
{
led_show(0, 255, 0); // green
if (initstatus == IS_OK)
led_show(0, 255, 0); // 绿色
else
{
Serial.println("not IS_OK");
led_show(255, 255, 0); // 黄色
}
}
else
{
if (motocontrol.getcontrolstatus().is_setzero)
led_show(255, 255, 255); // 紫色
led_show(255, 255, 255); // 白色
else
led_show(255, 255, 0); // yellow
{
Serial.println("not is_setzero");
led_show(255, 255, 0); // 黄色
}
}
break;
}
}
//超重--红色
if (motocontrol.getcontrolstatus().is_overweight)
led_show(255, 0, 0); // red
}
/////////////////////////////////////按钮处理
@ -427,5 +493,5 @@ void upbtn_pressend()
void testbtn_click()
{
Serial.println("testbtn_click");
motocontrol.moto_goodsdown();
motocontrol.moto_goodsdown(40);
}

146
src/motocontrol.cpp
View File

@ -21,7 +21,8 @@ Motocontrol::Motocontrol()
_hooksstatus = HS_UnInit;
_weightalign = false;
_overweightcount = 0;
_autogoodsdown=false;
_notweightcount = 0;
_autogoodsdown = false;
}
Motocontrol::~Motocontrol()
{
@ -40,19 +41,21 @@ void Motocontrol::setspeed(float motospeed) // 设置速度
void Motocontrol::setweight(int pullweight) // 设置重量
{
_pullweight = pullweight;
checkoverweight();
checkgoods();
}
void Motocontrol::checkoverweight() // 检测是否超重
void Motocontrol::checkgoods() // 检测是否超重
{
// 到顶部锁定状态，有向上的压力，重量不准，不能检测
if (_controlstatus.is_toplocked)
return;
// 检查是否超重
if (_pullweight > HOOK_WEIHT_MAX)
{
// 防止毛刺
_overweightcount++;
if (_overweightcount > 5)
if (_overweightcount > 40)
_controlstatus.is_overweight = true;
}
else
@ -60,6 +63,23 @@ void Motocontrol::checkoverweight() // 检测是否超重
_controlstatus.is_overweight = false;
_overweightcount = 0;
}
// 检查是否有货物
if (_pullweight < HOOK_WEIHT_MIN)
{
// 防止毛刺
_notweightcount++;
if (_notweightcount > 40)
{
// printf("goods weight<min 40 times :%d \n", _pullweight);
_controlstatus.is_havegoods = false;
}
}
else
{
_controlstatus.is_havegoods = true;
_notweightcount = 0;
}
}
void Motocontrol::stop() // 停止
{
@ -78,9 +98,10 @@ void Motocontrol::setlocked(bool locked)
{
stop();
setzero();
_hooksstatus = HS_Locked;
_autogoodsdown = false;
}
_controlstatus.is_toplocked = locked;
_hooksstatus = HS_Locked;
}
void Motocontrol::setzero() // 设置0长度位置
{
@ -92,19 +113,12 @@ int16_t Motocontrol::getlength() // 得到长度
{
return 0;
}
// 检测是否挂有货物
bool Motocontrol::havegoods()
{
return _pullweight > HOOK_WEIHT_MIN;
}
// 检测是否挂有货物
void Motocontrol::goodsstartup()
{
moto_run(MS_Stop);
setspeed(SPEED_HOOK_UP);
moto_run(MS_Up);
_hooksstatus = HS_Up;
_hooksstatus = HS_WaitUnhook;
_tm_waitunhook = millis();
}
// 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign)
@ -113,8 +127,9 @@ void Motocontrol::weightalign(bool weightalign)
}
void Motocontrol::checkhookstatus() // 检查挂钩状态
{
//需要在自动放货物中
if (!_autogoodsdown) return;
// 需要在自动放货物中
if (!_autogoodsdown)
return;
_hook_currlen = _curr_length;
switch (_hooksstatus)
@ -123,17 +138,25 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
case HookStatus::HS_Down:
{
// 如果没有货物--开始回收
if (!havegoods())
if (!_controlstatus.is_havegoods)
{
printf("HS_Down not havegoods wait unhook \n");
goodsstartup();
break;
}
break;
}
case HookStatus::HS_Stop:
{
// 如果有长度目标
if (_hook_targetlen > 0)
if ((_hook_targetlen > 0) && (_hook_targetlen <= HOOK_SLOWDOWN_LENGTH))
{
printf("_hook_targetlen>0 \n");
// 如果停了就开始慢速
if (_controlstatus.motostatus == MS_Stop)
{
printf("begin hook slow down \n");
// 慢速一直下放，直到货物卸下--没有重量
setspeed(SPEED_HOOK_SLOW);
moto_run(MS_Down);
@ -143,33 +166,45 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
else
{
// 没有长度目标,如果停了并且货物还在，一般是线到最大了，所有开始回收
if (_controlstatus.motostatus == MS_Stop)
{
printf("not settargetlen arrived wait unhook \n");
goodsstartup();
}
}
break;
}
case HookStatus::HS_DownSlow:
{
if (!havegoods())
if (!_controlstatus.is_havegoods)
{
printf("HS_DownSlow not havegoods wait unhook \n");
goodsstartup();
}
break;
}
case HookStatus::HS_WaitUnhook:
{
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{
printf("HS_WaitUnhook ok startup \n");
moto_run(MS_Stop);
setspeed(SPEED_HOOK_UP);
moto_run(MS_Up);
}
break;
}
case HookStatus::HS_Up:
{
if (_controlstatus.is_instore)
_hooksstatus = HS_InStore;
// 会自动设置HS_InStore状态
break;
}
case HookStatus::HS_InStore:
{
if (_controlstatus.is_toplocked)
{
_autogoodsdown=false;
_hooksstatus = HS_Locked;
}
// 如果到顶部，会触发setlocked,不用在这里设置
break;
}
}
@ -193,7 +228,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if ((_curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
{
_moto_dji.settarget(-SPEED_INSTORE);
_controlstatus.is_instore = true;
if (!_controlstatus.is_instore)
{
_controlstatus.is_instore = true;
_hooksstatus = HS_InStore;
printf("begin instore \n");
}
}
else
_controlstatus.is_instore = false;
@ -241,38 +281,52 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdown(float length)
{
if (length < 0.0)
{
printf("length<0 fault\n");
return;
}
// 没设置零点
if (!_controlstatus.is_setzero)
{
printf("not lengthzero fault\n");
return;
}
if (_weightalign)
if (!_weightalign)
{
printf("weight not align fault\n");
return;
}
// 没挂东西也没设置下放长度
if (!havegoods() && (length == 0.0))
// 没挂东西
if (!_controlstatus.is_havegoods )
{
printf("goods min fault:%d ,len:%.2f\n", _pullweight, length);
printf("goods min fault:%d \n", _pullweight);
return;
}
//开始自动放货物状态
_autogoodsdown=true;
// 开始自动放货物状态
_autogoodsdown = true;
_hook_targetlen = length;
// 长度=0，直接中慢速下降，
if (length == 0.0)
{
setspeed(SPEED_HOOK_CHECK);
else
setspeed(SPEED_HOOK_FAST);
if (length > 0)
moto_run(MS_Down, length - HOOK_SLOWDOWN_LENGTH);
else
moto_run(MS_Down);
_hooksstatus = HS_Down;
}
else if (length > HOOK_SLOWDOWN_LENGTH) // 长度大于慢速长度
{
setspeed(SPEED_HOOK_FAST);
moto_run(MS_Down, length - HOOK_SLOWDOWN_LENGTH);
_hook_targetlen = HOOK_SLOWDOWN_LENGTH;
}
else
{
setspeed(SPEED_HOOK_SLOW); // 长度小于慢速长度
moto_run(MS_Down, length);
}
}
// 按指定速度收放线 放线为+，收线为- 单位cm
void Motocontrol::moto_run(MotoStatus updown, float length)
{
@ -297,12 +351,20 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
}
if (updown == MS_Up)
{
if (_controlstatus.is_toplocked)
{
printf("is_toplocked return\n");
return;
}
if (_controlstatus.is_overweight)
{
printf("overweight fault :%d \n", _pullweight);
return;
}
_hooksstatus = HS_Up;
}
else
_hooksstatus = HS_Down;
_lockservo->write(SERVO_UNLOCKPOS);
_need_closemoto = false;

24
src/motocontrol.h
View File

@ -17,21 +17,22 @@
#define SPEED_BTN_MID 100 //按键中等收放线速度
#define SPEED_BTN_FAST 200 //按键最快收放线速度
#define SPEED_HOOK_FAST 200 //货物下放速度--有目标长度得情况下
#define SPEED_HOOK_FAST 100 //货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP 50 //空钩上升速度
#define SERVO_LOCKPOS 1900 //舵机锁定位置
#define SERVO_UNLOCKPOS 1500 //舵机解锁位置
#define SERVO_LOCKPOS 1820 //舵机锁定位置
#define SERVO_UNLOCKPOS 1600 //舵机解锁位置
#define HOOK_WEIHT_MIN 10 //最小货物重量 小于这个认为没挂东西 (克)
#define HOOK_WEIHT_MIN 100 //最小货物重量 小于这个认为没挂东西 (克)
#define HOOK_WEIHT_MAX 5000 //最大货物重量 大于这个认为超重不工作 (克)
#define HOOK_WEIHT_MAX 6000 //最大货物重量 大于这个认为超重不工作 (克)
#define HOOK_SLOWDOWN_LENGTH 50 //下放物体时快到目标长度时慢速长度 (cm)
#define HOOK_SLOWDOWN_LENGTH 10 //下放物体时快到目标长度时慢速长度 (cm)
#define HOOK_UNHOOK_TIME 1000 //等待脱钩时间ms
// 挂钩状态
enum MotoStatus
@ -47,6 +48,7 @@ typedef struct
bool is_setzero; //已设置0长度位置
bool is_toplocked; //已到顶锁住
bool is_overweight; //是否超重
bool is_havegoods; //是否有货物
float zero_cnt; //0长度位置
float speed_targe; //当前目标速度
MotoStatus motostatus; //电机运行状态
@ -75,12 +77,16 @@ private:
float _curr_length;
bool _weightalign;
bool _autogoodsdown;
u_int8_t _overweightcount;
uint8_t _overweightcount;
uint8_t _notweightcount;
unsigned long _tm_waitunhook;
void checkmotostatus();
void checkhookstatus();
bool havegoods();
void goodsstartup() ;
void checkoverweight();
void checkgoods();
public:
Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数