加入初始化操作和重量检测
打算重构,不要motostatus改为Hookstatus
This commit is contained in:
parent
15318bbe05
commit
50e470f135
@ -1,6 +1,7 @@
|
||||
#include "Serialcommand.h"
|
||||
#include "Arduino.h"
|
||||
#include "CRC.h"
|
||||
#include "CRC.h"
|
||||
#define HEAD_HI 0xA5
|
||||
#define HEAD_LI 0x5A
|
||||
#define DEBUG_OUT false
|
||||
|
||||
14
src/config.h
14
src/config.h
@ -9,6 +9,7 @@
|
||||
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
|
||||
#define BTN_DOWN 22 // 放线开关
|
||||
#define BTN_CT 21 // 到顶检测开关
|
||||
#define BTN_TEST 18 // 测试开关
|
||||
// LED
|
||||
#define LED_TIP 19 // 指示灯 接线：LED_TIP---3.3V
|
||||
// 称重传感器- HX711
|
||||
@ -18,13 +19,14 @@
|
||||
#define SPEED_UP_SLOW 4 //rmp 或者 弧度/秒
|
||||
#define SERVO_PIN 14 //锁定舵机PWM控制脚
|
||||
|
||||
|
||||
// 挂钩状态
|
||||
enum HookStatus
|
||||
{
|
||||
HS_Unknow, // 未知状态
|
||||
HS_Lock, // 已入仓并锁定
|
||||
HS_Down, // 自动下放中
|
||||
HS_UnInit, //还未初始化
|
||||
HS_Down, // 货物下放中
|
||||
HS_DownSlow, //货物慢速下放中
|
||||
HS_Up, // 回收中
|
||||
HS_Inbox // 缓慢入仓中(货物重量测量相对准确)
|
||||
HS_InStore, // 入仓中
|
||||
HS_Locked, //已到顶部锁定
|
||||
HS_Stop //已停止
|
||||
};
|
||||
|
||||
|
||||
254
src/main.cpp
254
src/main.cpp
@ -9,7 +9,7 @@
|
||||
#include "moto.h"
|
||||
#include <FastLED.h>
|
||||
#include <ESP32Servo.h>
|
||||
#define TM_INSTORE_DELAY 100 //入仓延时关闭时间 ms
|
||||
#define TM_INSTORE_DELAY 100 // 入仓延时关闭时间 ms
|
||||
|
||||
// 角度传感器
|
||||
// 收放线电机控制
|
||||
@ -19,9 +19,12 @@
|
||||
OneButton button_up(BTN_UP, true);
|
||||
OneButton button_down(BTN_DOWN, true);
|
||||
OneButton button_checktop(BTN_CT, true);
|
||||
|
||||
OneButton button_test(BTN_TEST, true);
|
||||
|
||||
HX711 scale;
|
||||
Serialcommand sercomm;
|
||||
Servo myservo;
|
||||
Servo myservo;
|
||||
#define NUM_LEDS 1
|
||||
#define DATA_PIN 25
|
||||
CRGB leds[NUM_LEDS];
|
||||
@ -30,8 +33,6 @@ Motocontrol motocontrol;
|
||||
void sendinfo();
|
||||
Ticker tksendinfo(sendinfo, 500); // 发送状态
|
||||
|
||||
HookStatus hookstatus = HS_Unknow;
|
||||
|
||||
// 收
|
||||
void upbtn_click();
|
||||
void upbtn_dbclick();
|
||||
@ -47,15 +48,29 @@ void ctbtn_pressend();
|
||||
// 顶按下
|
||||
void ctbtn_pressstart();
|
||||
|
||||
void testbtn_click();
|
||||
|
||||
int get_pullweight();
|
||||
void Task1(void *pvParameters);
|
||||
void led_show(CRGB ledcolor);
|
||||
// void Task1code( void *pvParameters );
|
||||
void showledidel();
|
||||
int pullweight = 0;
|
||||
unsigned long _tm_bengstop;
|
||||
bool _bengstop=false;
|
||||
unsigned long _tm_bengstop;
|
||||
bool _bengstop = false;
|
||||
|
||||
// 需要做的初始化工作
|
||||
enum Initstatus
|
||||
{
|
||||
IS_Start, // 0. 刚上电
|
||||
IS_Wait_LengthZero,
|
||||
IS_LengthZero, // 1.已确定零点
|
||||
IS_Wait_WeightZero,
|
||||
IS_WeightZero, // 2.已确定称重传感器0点
|
||||
IS_InStoreLocked, // 3. 挂钩入仓顶锁定
|
||||
IS_OK, // 4.所有初始化已经OK，可以正常操作
|
||||
IS_Error // 5.初始化遇到错误
|
||||
} initstatus;
|
||||
|
||||
void setup()
|
||||
{
|
||||
@ -82,22 +97,23 @@ void setup()
|
||||
button_checktop.attachLongPressStart(ctbtn_pressstart); // 按下马上产生事件，
|
||||
button_checktop.attachLongPressStop(ctbtn_pressend); // 抬起
|
||||
|
||||
ESP32PWM::allocateTimer(0);
|
||||
ESP32PWM::allocateTimer(1);
|
||||
ESP32PWM::allocateTimer(2);
|
||||
ESP32PWM::allocateTimer(3);
|
||||
myservo.setPeriodHertz(50); // standard 50 hz servo
|
||||
myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object
|
||||
button_test.attachClick(testbtn_click);
|
||||
|
||||
ESP32PWM::allocateTimer(0);
|
||||
ESP32PWM::allocateTimer(1);
|
||||
ESP32PWM::allocateTimer(2);
|
||||
ESP32PWM::allocateTimer(3);
|
||||
myservo.setPeriodHertz(50); // standard 50 hz servo
|
||||
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
|
||||
if (!motocontrol.init(&myservo)) // 初始化电机控制
|
||||
if (!motocontrol.init(&myservo)) // 初始化电机控制
|
||||
printf("motocontrol init fault\n");
|
||||
|
||||
// 发送状态任务开启--500ms一次
|
||||
tksendinfo.start();
|
||||
|
||||
initstatus = IS_Start;
|
||||
// if (motocontrol.getstatus()==MS_Stop)
|
||||
// {
|
||||
// //slowup();
|
||||
@ -115,10 +131,75 @@ 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);
|
||||
//if (pullweight > 10)
|
||||
// printf("PullWeight:%d\n", pullweight);
|
||||
}
|
||||
// 初始化过程--
|
||||
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
|
||||
//
|
||||
void checkinited()
|
||||
{
|
||||
switch (initstatus)
|
||||
{
|
||||
case Initstatus::IS_Start:
|
||||
{
|
||||
// 开始自动慢速上升，直到顶部按钮按下
|
||||
printf("IS_Start: startup wait locked\n");
|
||||
motocontrol.setspeed(SPEED_BTN_SLOW);
|
||||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||||
initstatus = IS_Wait_LengthZero;
|
||||
break;
|
||||
}
|
||||
case Initstatus::IS_Wait_LengthZero:
|
||||
{
|
||||
if (motocontrol.gethooktatus() == HS_Locked)
|
||||
{
|
||||
printf("IS_Wait_LengthZero: is locked\n");
|
||||
initstatus = IS_LengthZero;
|
||||
}
|
||||
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上执行，重要的延迟低的
|
||||
void loop()
|
||||
{
|
||||
@ -127,16 +208,19 @@ void loop()
|
||||
button_checktop.tick(); // 按钮
|
||||
button_down.tick(); // 按钮
|
||||
button_up.tick(); // 按钮
|
||||
motocontrol.update(); // 电机控制
|
||||
showledidel(); //显示LED灯光
|
||||
showinfo(); // 显示一些调试用信息
|
||||
if ((_bengstop)&&((millis()-_tm_bengstop)>TM_INSTORE_DELAY))
|
||||
button_test.tick();
|
||||
motocontrol.setweight(pullweight); // 告诉电机拉的重量
|
||||
motocontrol.update(); // 电机控制
|
||||
showledidel(); // 显示LED灯光
|
||||
showinfo(); // 显示一些调试用信息
|
||||
// 到顶后延迟关闭动力电和舵机
|
||||
if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY))
|
||||
{
|
||||
_bengstop=false;
|
||||
_bengstop = false;
|
||||
motocontrol.setlocked(true);
|
||||
hookstatus = HS_Lock;
|
||||
}
|
||||
|
||||
// 检测执行初始化工作
|
||||
checkinited();
|
||||
delay(1);
|
||||
}
|
||||
// 在核心0上执行耗时长的低优先级的
|
||||
@ -144,7 +228,7 @@ void Task1(void *pvParameters)
|
||||
{
|
||||
// 初始化称重传感器
|
||||
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
|
||||
scale.set_scale(516.f); // 这是缩放值，根据砝码实测
|
||||
scale.set_scale(963.f); // 这是缩放值，根据砝码实测516.f
|
||||
scale.tare(); // 重置为0
|
||||
|
||||
// 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
|
||||
@ -156,7 +240,7 @@ void Task1(void *pvParameters)
|
||||
}
|
||||
void sendinfo() // 每500ms发送状态信息
|
||||
{
|
||||
sercomm.sendinfo(hookstatus, 5324, pullweight);
|
||||
sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
|
||||
}
|
||||
|
||||
float Value;
|
||||
@ -174,9 +258,9 @@ int get_pullweight()
|
||||
return round(Value);
|
||||
}
|
||||
// 提示灯光控制
|
||||
void led_show(uint8_t cr,uint8_t cg,uint8_t cb)
|
||||
void led_show(uint8_t cr, uint8_t cg, uint8_t cb)
|
||||
{
|
||||
leds[0] =CRGB(cg,cr,cb);//颜色交叉了
|
||||
leds[0] = CRGB(cg, cr, cb); // 颜色交叉了
|
||||
FastLED.show();
|
||||
}
|
||||
// 显示颜色
|
||||
@ -186,17 +270,17 @@ void showledidel()
|
||||
{
|
||||
case MotoStatus::MS_Down:
|
||||
{
|
||||
led_show(0,0,255); //blue
|
||||
led_show(0, 0, 255); // blue
|
||||
|
||||
break;
|
||||
}
|
||||
case MotoStatus::MS_Up:
|
||||
{
|
||||
|
||||
|
||||
if (motocontrol.getcontrolstatus().is_instore)
|
||||
led_show(255,0,0); // red
|
||||
led_show(255, 0, 0); // red
|
||||
else
|
||||
led_show(200,0,50); // red
|
||||
led_show(200, 0, 50); // red
|
||||
break;
|
||||
}
|
||||
|
||||
@ -204,17 +288,22 @@ void showledidel()
|
||||
{
|
||||
if (motocontrol.getcontrolstatus().is_toplocked)
|
||||
{
|
||||
led_show(0,255,0); //green
|
||||
}else
|
||||
led_show(0, 255, 0); // green
|
||||
}
|
||||
else
|
||||
{
|
||||
if (motocontrol.getcontrolstatus().is_setzero)
|
||||
led_show(255,255,255); // 紫色
|
||||
led_show(255, 255, 255); // 紫色
|
||||
else
|
||||
led_show(255,255,0); //yellow
|
||||
led_show(255, 255, 0); // yellow
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//超重--红色
|
||||
if (motocontrol.getcontrolstatus().is_overweight)
|
||||
led_show(255, 0, 0); // red
|
||||
}
|
||||
|
||||
/////////////////////////////////////按钮处理
|
||||
@ -222,46 +311,34 @@ void showledidel()
|
||||
void ctbtn_pressstart()
|
||||
{
|
||||
Serial.println("ctbtn_pressstart");
|
||||
|
||||
_tm_bengstop=millis();
|
||||
_bengstop=true;
|
||||
|
||||
_tm_bengstop = millis();
|
||||
_bengstop = true;
|
||||
}
|
||||
|
||||
// 顶部按钮，抬起
|
||||
void ctbtn_pressend()
|
||||
{
|
||||
Serial.println("ctbtn_pressend");
|
||||
motocontrol.setlocked(false);
|
||||
_bengstop=false;
|
||||
_bengstop = false;
|
||||
}
|
||||
|
||||
// 放线按钮--单击
|
||||
void downbtn_click()
|
||||
{
|
||||
Serial.println("downbtn_click");
|
||||
|
||||
if (motocontrol.getcontrolstatus().motostatus!= MotoStatus::MS_Stop)
|
||||
{
|
||||
motocontrol.stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_SLOW);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down);
|
||||
}
|
||||
}
|
||||
void upbtn_click()
|
||||
{
|
||||
Serial.println("upbtn_click");
|
||||
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
|
||||
{
|
||||
motocontrol.stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_SLOW);
|
||||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||||
motocontrol.setspeed(SPEED_BTN_SLOW);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down);
|
||||
}
|
||||
}
|
||||
|
||||
// 放线按钮--双击
|
||||
void downbtn_dbclick()
|
||||
{
|
||||
Serial.println("downbtn_dbclick");
|
||||
@ -272,10 +349,46 @@ void downbtn_dbclick()
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_FAST);
|
||||
motocontrol.setspeed(SPEED_BTN_MID);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down);
|
||||
}
|
||||
}
|
||||
// 放线按钮--长按
|
||||
void downbtn_pressstart()
|
||||
{
|
||||
Serial.println("downbtn_pressstart");
|
||||
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
|
||||
{
|
||||
motocontrol.stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_BTN_FAST);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down);
|
||||
}
|
||||
}
|
||||
// 放线按钮--长按抬起
|
||||
void downbtn_pressend()
|
||||
{
|
||||
Serial.println("downbtn_pressend");
|
||||
motocontrol.stop();
|
||||
}
|
||||
|
||||
// 收线按钮-单击
|
||||
void upbtn_click()
|
||||
{
|
||||
Serial.println("upbtn_click");
|
||||
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
|
||||
{
|
||||
motocontrol.stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_BTN_SLOW);
|
||||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||||
}
|
||||
}
|
||||
// 收线按钮-双击
|
||||
void upbtn_dbclick()
|
||||
{
|
||||
Serial.println("upbtn_dbclick");
|
||||
@ -285,11 +398,11 @@ void upbtn_dbclick()
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_FAST);
|
||||
motocontrol.setspeed(SPEED_BTN_MID);
|
||||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||||
}
|
||||
}
|
||||
|
||||
// 收线按钮-长按
|
||||
void upbtn_pressstart()
|
||||
{
|
||||
Serial.println("upbtn_pressstart");
|
||||
@ -303,27 +416,16 @@ void upbtn_pressstart()
|
||||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||||
}
|
||||
}
|
||||
// 收线按钮-长按抬起
|
||||
void upbtn_pressend()
|
||||
{
|
||||
Serial.println("upbtn_pressend");
|
||||
motocontrol.stop();
|
||||
}
|
||||
|
||||
void downbtn_pressstart()
|
||||
// 测试按钮
|
||||
void testbtn_click()
|
||||
{
|
||||
Serial.println("downbtn_pressstart");
|
||||
if (motocontrol.getcontrolstatus().motostatus!= MotoStatus::MS_Stop)
|
||||
{
|
||||
motocontrol.stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
motocontrol.setspeed(SPEED_BTN_FAST);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down);
|
||||
}
|
||||
}
|
||||
void downbtn_pressend()
|
||||
{
|
||||
Serial.println("downbtn_pressend");
|
||||
motocontrol.stop();
|
||||
}
|
||||
Serial.println("testbtn_click");
|
||||
motocontrol.moto_goodsdown();
|
||||
}
|
||||
@ -3,17 +3,25 @@
|
||||
#include "motocontrol.h"
|
||||
#include "moto.h"
|
||||
#include <ESP32Servo.h>
|
||||
#include "config.h"
|
||||
|
||||
#define DEBUG_OUT false
|
||||
Motocontrol::Motocontrol()
|
||||
{
|
||||
_controlstatus.is_instore = false;
|
||||
_controlstatus.is_setzero = false;
|
||||
_controlstatus.is_overweight = false;
|
||||
_controlstatus.zero_cnt = 0;
|
||||
_controlstatus.motostatus = MotoStatus::MS_Stop;
|
||||
_controlstatus.speed_targe = SPEED_SLOW;
|
||||
_controlstatus.speed_targe = SPEED_BTN_SLOW;
|
||||
_check_targetlength = false;
|
||||
_need_closemoto=false;
|
||||
_need_closemoto = false;
|
||||
_pullweight = 0;
|
||||
_curr_length = 0.0;
|
||||
_hooksstatus = HS_UnInit;
|
||||
_weightalign = false;
|
||||
_overweightcount = 0;
|
||||
_autogoodsdown=false;
|
||||
}
|
||||
Motocontrol::~Motocontrol()
|
||||
{
|
||||
@ -21,7 +29,7 @@ Motocontrol::~Motocontrol()
|
||||
|
||||
bool Motocontrol::init(Servo *lockservo) // 初始化
|
||||
{
|
||||
_lockservo=lockservo;
|
||||
_lockservo = lockservo;
|
||||
return _moto_dji.init();
|
||||
}
|
||||
void Motocontrol::setspeed(float motospeed) // 设置速度
|
||||
@ -29,24 +37,50 @@ void Motocontrol::setspeed(float motospeed) // 设置速度
|
||||
_controlstatus.speed_targe = motospeed;
|
||||
}
|
||||
|
||||
void Motocontrol::setweight(int pullweight) // 设置重量
|
||||
{
|
||||
_pullweight = pullweight;
|
||||
checkoverweight();
|
||||
}
|
||||
|
||||
void Motocontrol::checkoverweight() // 检测是否超重
|
||||
{
|
||||
// 到顶部锁定状态，有向上的压力，重量不准，不能检测
|
||||
if (_controlstatus.is_toplocked)
|
||||
return;
|
||||
if (_pullweight > HOOK_WEIHT_MAX)
|
||||
{
|
||||
// 防止毛刺
|
||||
_overweightcount++;
|
||||
if (_overweightcount > 5)
|
||||
_controlstatus.is_overweight = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
_controlstatus.is_overweight = false;
|
||||
_overweightcount = 0;
|
||||
}
|
||||
}
|
||||
void Motocontrol::stop() // 停止
|
||||
{
|
||||
_moto_dji.settarget(0.0f);
|
||||
_controlstatus.motostatus = MS_Stop;
|
||||
_lockservo->write(SERVO_LOCKPOS);
|
||||
_need_closemoto=true;
|
||||
_tm_closemoto=millis();
|
||||
printf("write servo :%d \n",SERVO_LOCKPOS);
|
||||
_hooksstatus = HS_Stop;
|
||||
_lockservo->write(SERVO_LOCKPOS);
|
||||
_need_closemoto = true;
|
||||
_tm_closemoto = millis();
|
||||
printf("write servo :%d \n", SERVO_LOCKPOS);
|
||||
printf("stop \n");
|
||||
}
|
||||
void Motocontrol::setlocked(bool locked)
|
||||
{
|
||||
if (locked)
|
||||
{
|
||||
stop();
|
||||
setzero();
|
||||
}
|
||||
_controlstatus.is_toplocked=locked;
|
||||
if (locked)
|
||||
{
|
||||
stop();
|
||||
setzero();
|
||||
}
|
||||
_controlstatus.is_toplocked = locked;
|
||||
_hooksstatus = HS_Locked;
|
||||
}
|
||||
void Motocontrol::setzero() // 设置0长度位置
|
||||
{
|
||||
@ -58,26 +92,107 @@ int16_t Motocontrol::getlength() // 得到长度
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
void Motocontrol::checkstatus() // 检查状态，比如什么时候停
|
||||
// 检测是否挂有货物
|
||||
bool Motocontrol::havegoods()
|
||||
{
|
||||
//1秒后断电
|
||||
if ((_need_closemoto)&&((millis()-_tm_closemoto)>1000))
|
||||
_moto_dji.close();
|
||||
|
||||
return _pullweight > HOOK_WEIHT_MIN;
|
||||
}
|
||||
|
||||
// 检测是否挂有货物
|
||||
void Motocontrol::goodsstartup()
|
||||
{
|
||||
moto_run(MS_Stop);
|
||||
setspeed(SPEED_HOOK_UP);
|
||||
moto_run(MS_Up);
|
||||
_hooksstatus = HS_Up;
|
||||
}
|
||||
// 重量传感器已经校准
|
||||
void Motocontrol::weightalign(bool weightalign)
|
||||
{
|
||||
_weightalign = weightalign;
|
||||
}
|
||||
void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
{
|
||||
//需要在自动放货物中
|
||||
if (!_autogoodsdown) return;
|
||||
|
||||
_hook_currlen = _curr_length;
|
||||
switch (_hooksstatus)
|
||||
{
|
||||
// 正在下放货物
|
||||
case HookStatus::HS_Down:
|
||||
{
|
||||
// 如果没有货物--开始回收
|
||||
if (!havegoods())
|
||||
{
|
||||
goodsstartup();
|
||||
break;
|
||||
}
|
||||
// 如果有长度目标
|
||||
if (_hook_targetlen > 0)
|
||||
{
|
||||
// 如果停了就开始慢速
|
||||
if (_controlstatus.motostatus == MS_Stop)
|
||||
{
|
||||
// 慢速一直下放，直到货物卸下--没有重量
|
||||
setspeed(SPEED_HOOK_SLOW);
|
||||
moto_run(MS_Down);
|
||||
_hooksstatus = HS_DownSlow;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 没有长度目标,如果停了并且货物还在，一般是线到最大了，所有开始回收
|
||||
if (_controlstatus.motostatus == MS_Stop)
|
||||
goodsstartup();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case HookStatus::HS_DownSlow:
|
||||
{
|
||||
if (!havegoods())
|
||||
{
|
||||
goodsstartup();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case HookStatus::HS_Up:
|
||||
{
|
||||
if (_controlstatus.is_instore)
|
||||
_hooksstatus = HS_InStore;
|
||||
break;
|
||||
}
|
||||
case HookStatus::HS_InStore:
|
||||
{
|
||||
if (_controlstatus.is_toplocked)
|
||||
{
|
||||
_autogoodsdown=false;
|
||||
_hooksstatus = HS_Locked;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候停
|
||||
{
|
||||
// 1秒后断电，防止电调堵转保护
|
||||
if ((_need_closemoto) && ((millis() - _tm_closemoto) > 1000))
|
||||
_moto_dji.close();
|
||||
|
||||
moto_measure_t mf = _moto_dji.getmotoinfo();
|
||||
// 已设置零点
|
||||
if (_controlstatus.is_setzero)
|
||||
{
|
||||
// 总放线长度
|
||||
float 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
|
||||
if ((curr_length > ROPE_MAXLENGTH)&&(_controlstatus.motostatus == MS_Down))
|
||||
if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
|
||||
stop();
|
||||
// 开始入仓
|
||||
if ((curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
|
||||
if ((_curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
|
||||
{
|
||||
_moto_dji.settarget(-SPEED_MIN);
|
||||
_moto_dji.settarget(-SPEED_INSTORE);
|
||||
_controlstatus.is_instore = true;
|
||||
}
|
||||
else
|
||||
@ -107,7 +222,8 @@ void Motocontrol::checkstatus() // 检查状态，比如什么时候停
|
||||
void Motocontrol::update() // 更新
|
||||
{
|
||||
_moto_dji.update();
|
||||
checkstatus();
|
||||
checkmotostatus();
|
||||
checkhookstatus();
|
||||
}
|
||||
void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
|
||||
{
|
||||
@ -122,6 +238,41 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
|
||||
{
|
||||
return _controlstatus;
|
||||
}
|
||||
|
||||
void Motocontrol::moto_goodsdown(float length)
|
||||
{
|
||||
// 没设置零点
|
||||
if (!_controlstatus.is_setzero)
|
||||
{
|
||||
printf("not lengthzero fault\n");
|
||||
return;
|
||||
}
|
||||
if (_weightalign)
|
||||
{
|
||||
printf("weight not align fault\n");
|
||||
return;
|
||||
}
|
||||
// 没挂东西也没设置下放长度
|
||||
if (!havegoods() && (length == 0.0))
|
||||
{
|
||||
printf("goods min fault:%d ,len:%.2f\n", _pullweight, length);
|
||||
return;
|
||||
}
|
||||
|
||||
//开始自动放货物状态
|
||||
_autogoodsdown=true;
|
||||
|
||||
_hook_targetlen = length;
|
||||
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;
|
||||
}
|
||||
// 按指定速度收放线 放线为+，收线为- 单位cm
|
||||
void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
{
|
||||
@ -134,15 +285,29 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
|
||||
// 运动中暂时不管
|
||||
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
|
||||
{
|
||||
printf("motostatus is MS_Down\\MS_Up \n");
|
||||
return;
|
||||
}
|
||||
// 没设置速度不转
|
||||
if (_controlstatus.speed_targe == 0)
|
||||
{
|
||||
printf("not set speed_targe \n");
|
||||
return;
|
||||
}
|
||||
if (updown == MS_Up)
|
||||
{
|
||||
if (_controlstatus.is_overweight)
|
||||
{
|
||||
printf("overweight fault :%d \n", _pullweight);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_lockservo->write(SERVO_UNLOCKPOS);
|
||||
_need_closemoto=false;
|
||||
_lockservo->write(SERVO_UNLOCKPOS);
|
||||
_need_closemoto = false;
|
||||
|
||||
printf("write servo :%d \n",SERVO_UNLOCKPOS);
|
||||
printf("write servo :%d \n", SERVO_UNLOCKPOS);
|
||||
_controlstatus.motostatus = updown;
|
||||
// 有长度限制
|
||||
if (length != 0)
|
||||
@ -166,7 +331,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
{
|
||||
// 如果没有设置零点，不能快速收线
|
||||
if (!_controlstatus.is_setzero)
|
||||
_controlstatus.speed_targe = SPEED_SLOW;
|
||||
_controlstatus.speed_targe = SPEED_BTN_SLOW;
|
||||
runspeed = -_controlstatus.speed_targe;
|
||||
}
|
||||
// 开始转
|
||||
|
||||
@ -3,40 +3,53 @@
|
||||
#include "config.h"
|
||||
#include "moto.h"
|
||||
#include <ESP32Servo.h>
|
||||
|
||||
#define ROPE_MAXLENGTH 600 //最多能放600cm---实际绳子应该比这个长650之类的
|
||||
#define WHEEL_DIAMETER 4.0 //轮子直径cm
|
||||
#define INSTORE_LENGTH 10 //入仓长度,低于该长度要缓慢入仓 cm
|
||||
|
||||
#define SPEED_MAX 330 //最快收放线速度
|
||||
#define SPEED_MIN 5 //入仓速度
|
||||
#define SPEED_SLOW 20 //慢收放线速度
|
||||
#define SPEED_FAST 100 //快收放线速度
|
||||
#define SPEED_BTN_FAST 200 //最快收放线速度
|
||||
|
||||
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长
|
||||
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数
|
||||
|
||||
#define INSTORE_LENGTH 10 //入仓长度,低于该长度要缓慢入仓 cm
|
||||
|
||||
#define SPEED_MAX 330 //最快收放线速度--任何时候不应该超过这个速度
|
||||
#define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm
|
||||
#define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
|
||||
#define SPEED_BTN_MID 100 //按键中等收放线速度
|
||||
#define SPEED_BTN_FAST 200 //按键最快收放线速度
|
||||
|
||||
#define SPEED_HOOK_FAST 200 //货物下放速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
|
||||
#define SPEED_HOOK_UP 50 //空钩上升速度
|
||||
|
||||
|
||||
|
||||
#define SERVO_LOCKPOS 1900 //舵机锁定位置
|
||||
#define SERVO_UNLOCKPOS 1500 //舵机解锁位置
|
||||
|
||||
#define HOOK_WEIHT_MIN 10 //最小货物重量 小于这个认为没挂东西 (克)
|
||||
|
||||
#define HOOK_WEIHT_MAX 5000 //最大货物重量 大于这个认为超重不工作 (克)
|
||||
|
||||
#define HOOK_SLOWDOWN_LENGTH 50 //下放物体时快到目标长度时慢速长度 (cm)
|
||||
|
||||
// 挂钩状态
|
||||
enum MotoStatus
|
||||
{
|
||||
MS_Down, // 自动下放中
|
||||
MS_Up, // 回收中
|
||||
|
||||
MS_Stop //停止中
|
||||
};
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool is_instore;
|
||||
bool is_setzero;
|
||||
bool is_toplocked;
|
||||
float zero_cnt;
|
||||
float speed_targe;
|
||||
MotoStatus motostatus;
|
||||
bool is_instore; //正在进仓
|
||||
bool is_setzero; //已设置0长度位置
|
||||
bool is_toplocked; //已到顶锁住
|
||||
bool is_overweight; //是否超重
|
||||
float zero_cnt; //0长度位置
|
||||
float speed_targe; //当前目标速度
|
||||
MotoStatus motostatus; //电机运行状态
|
||||
|
||||
} control_status_t;
|
||||
|
||||
@ -55,8 +68,19 @@ private:
|
||||
bool _check_targetlength;
|
||||
bool _need_closemoto;
|
||||
unsigned long _tm_closemoto;
|
||||
|
||||
void checkstatus();
|
||||
int _pullweight;
|
||||
HookStatus _hooksstatus;
|
||||
int _hook_targetlen;
|
||||
int _hook_currlen;
|
||||
float _curr_length;
|
||||
bool _weightalign;
|
||||
bool _autogoodsdown;
|
||||
u_int8_t _overweightcount;
|
||||
void checkmotostatus();
|
||||
void checkhookstatus();
|
||||
bool havegoods();
|
||||
void goodsstartup() ;
|
||||
void checkoverweight();
|
||||
public:
|
||||
Motocontrol(); // 构造函数
|
||||
~Motocontrol(); // 析构函数
|
||||
@ -70,7 +94,10 @@ public:
|
||||
moto_measure_t getmotoinfo(); //得到电机信息
|
||||
control_status_t getcontrolstatus(); //得到控制信息
|
||||
void setlocked(bool locked); //是否到顶锁定
|
||||
void moto_run(MotoStatus updown,float length= 0.0f) ;// 放线
|
||||
|
||||
void moto_run(MotoStatus updown,float length= 0.0f) ;// 收放线
|
||||
void setweight(int pullweight);//设置货物实时重量
|
||||
void moto_goodsdown(float length= 0.0f);// 放下货物,自动回收
|
||||
HookStatus gethooktatus(){return _hooksstatus;} //得到挂钩状态
|
||||
void weightalign(bool weightalign); //重量传感器是否校准--必须校准才能执行moto_hookdown
|
||||
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user