仅格式化代码，没有任何其他修改
This commit is contained in:
parent
5f414479f1
commit
93ec10391a
@ -28,7 +28,8 @@ void Serialcommand::Uart_Command_Rev(void)
|
||||
while (Serial2.available() > 0)
|
||||
{
|
||||
ch = Serial2.read();
|
||||
if (DEBUG_OUT) printf("rev:%d\n", ch);
|
||||
if (DEBUG_OUT)
|
||||
printf("rev:%d\n", ch);
|
||||
switch (CmdState)
|
||||
{
|
||||
// 开始标志1
|
||||
@ -37,7 +38,8 @@ void Serialcommand::Uart_Command_Rev(void)
|
||||
if (HEAD_HI == ch)
|
||||
{
|
||||
CmdState = 1;
|
||||
if (DEBUG_OUT) printf("st:%d\n", CmdState);
|
||||
if (DEBUG_OUT)
|
||||
printf("st:%d\n", CmdState);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -48,7 +50,8 @@ void Serialcommand::Uart_Command_Rev(void)
|
||||
{
|
||||
bufferIndex = 0;
|
||||
CmdState = 2;
|
||||
if (DEBUG_OUT) printf("st:%d\n", CmdState);
|
||||
if (DEBUG_OUT)
|
||||
printf("st:%d\n", CmdState);
|
||||
}
|
||||
else
|
||||
CmdState = 0;
|
||||
@ -58,13 +61,15 @@ void Serialcommand::Uart_Command_Rev(void)
|
||||
case 2:
|
||||
{
|
||||
datalen = ch;
|
||||
if (DEBUG_OUT) printf("datalen:%d\n", datalen);
|
||||
if (DEBUG_OUT)
|
||||
printf("datalen:%d\n", datalen);
|
||||
if (datalen <= inbuffersize)
|
||||
{
|
||||
CmdState = 3;
|
||||
bufferIndex = 0;
|
||||
inBuffer[bufferIndex] = ch;
|
||||
if (DEBUG_OUT) printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
|
||||
if (DEBUG_OUT)
|
||||
printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
|
||||
}
|
||||
else // 超长
|
||||
{
|
||||
@ -75,13 +80,15 @@ void Serialcommand::Uart_Command_Rev(void)
|
||||
// 收数据直到结束
|
||||
case 3:
|
||||
{
|
||||
if (DEBUG_OUT) printf("st3:%d:%d\n", bufferIndex, datalen);
|
||||
if (DEBUG_OUT)
|
||||
printf("st3:%d:%d\n", bufferIndex, datalen);
|
||||
|
||||
if (bufferIndex <= datalen)
|
||||
{
|
||||
bufferIndex++;
|
||||
inBuffer[bufferIndex] = ch;
|
||||
if (DEBUG_OUT) printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
|
||||
if (DEBUG_OUT)
|
||||
printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
|
||||
if (bufferIndex == datalen) // 收完了
|
||||
{
|
||||
CmdState = 0;
|
||||
@ -89,25 +96,27 @@ void Serialcommand::Uart_Command_Rev(void)
|
||||
bufferIndex = 0;
|
||||
return; // 不收数据了，先出去解析
|
||||
}
|
||||
} else //异常
|
||||
}
|
||||
else // 异常
|
||||
{
|
||||
if (DEBUG_OUT) printf("buf error:%d:%d\n", bufferIndex, datalen);
|
||||
if (DEBUG_OUT)
|
||||
printf("buf error:%d:%d\n", bufferIndex, datalen);
|
||||
CmdState = 0;
|
||||
bufferIndex = 0;
|
||||
recv_flag=false;
|
||||
return;
|
||||
recv_flag = false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//CRC8
|
||||
//多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107）
|
||||
//https://zhuanlan.zhihu.com/p/114049042
|
||||
// CRC8
|
||||
// 多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107）
|
||||
// https://zhuanlan.zhihu.com/p/114049042
|
||||
uint8_t Serialcommand::getsum(const uint8_t *buffer, size_t size)
|
||||
{
|
||||
return crc8(buffer, size, 0x07, 0x00, 0x00, false, false);
|
||||
return crc8(buffer, size, 0x07, 0x00, 0x00, false, false);
|
||||
}
|
||||
bool Serialcommand::checksum()
|
||||
{
|
||||
@ -119,23 +128,22 @@ bool Serialcommand::checksum()
|
||||
}
|
||||
return bufsum == chksum;
|
||||
}
|
||||
void Serialcommand::sendinfo(HookStatus status,int linelength,int pullweight )
|
||||
void Serialcommand::sendinfo(HookStatus status, int linelength, int pullweight)
|
||||
{
|
||||
|
||||
// 长度
|
||||
outBuffer[2] = 0x07;
|
||||
outBuffer[2] = 0x07;
|
||||
outBuffer[3] = 0x02;
|
||||
outBuffer[4] = status;
|
||||
|
||||
outBuffer[5] =(uint8_t) ((linelength>>8) &0xff);
|
||||
outBuffer[6] = (uint8_t) (linelength&0xff);
|
||||
outBuffer[5] = (uint8_t)((linelength >> 8) & 0xff);
|
||||
outBuffer[6] = (uint8_t)(linelength & 0xff);
|
||||
|
||||
outBuffer[7] =(uint8_t) ((pullweight>>8) &0xff);
|
||||
outBuffer[8] = (uint8_t) (pullweight&0xff);
|
||||
outBuffer[7] = (uint8_t)((pullweight >> 8) & 0xff);
|
||||
outBuffer[8] = (uint8_t)(pullweight & 0xff);
|
||||
|
||||
outBuffer[9] = getsum(outBuffer + 2, 7);
|
||||
Serial2.write(outBuffer, 10);
|
||||
|
||||
}
|
||||
|
||||
void Serialcommand::sendcmdret(uint8_t cmdret)
|
||||
@ -155,7 +163,8 @@ void Serialcommand::getcommand()
|
||||
// 收到有效数据
|
||||
if (recv_flag)
|
||||
{
|
||||
if (DEBUG_OUT) printf("rev end len:%d\n", datalen);
|
||||
if (DEBUG_OUT)
|
||||
printf("rev end len:%d\n", datalen);
|
||||
recv_flag = false;
|
||||
// 处理数据
|
||||
if (checksum())
|
||||
|
||||
@ -22,5 +22,5 @@ public:
|
||||
Serialcommand(); // 构造函数
|
||||
~Serialcommand(); // 析构函数
|
||||
void getcommand();
|
||||
void sendinfo(HookStatus status,int linelength,int pullweight );
|
||||
void sendinfo(HookStatus status, int linelength, int pullweight);
|
||||
};
|
||||
|
||||
37
src/config.h
37
src/config.h
@ -1,37 +1,36 @@
|
||||
#pragma once
|
||||
////////////
|
||||
//定义公共结构，变量，硬件接口等
|
||||
// 定义公共结构，变量，硬件接口等
|
||||
///
|
||||
//
|
||||
|
||||
//硬件接口定义////////////////////////////
|
||||
// 按钮
|
||||
// 硬件接口定义////////////////////////////
|
||||
// 按钮
|
||||
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
|
||||
#define BTN_DOWN 22 // 放线开关
|
||||
#define BTN_CT 21 // 到顶检测开关
|
||||
#define BTN_TEST 18 // 测试开关
|
||||
#define BTN_TEST 18 // 测试开关
|
||||
// 称重传感器- HX711
|
||||
#define LOADCELL_DOUT_PIN 16
|
||||
#define LOADCELL_SCK_PIN 17
|
||||
#define LOADCELL_DOUT_PIN 16
|
||||
#define LOADCELL_SCK_PIN 17
|
||||
///////////////////////////////////////////
|
||||
#define SERVO_PIN 14 //锁定舵机PWM控制脚
|
||||
#define SERVO_PIN 14 // 锁定舵机PWM控制脚
|
||||
////LED
|
||||
#define LED_DATA_PIN 25
|
||||
//Moto-CAN
|
||||
#define MOTO_CAN_RX 26
|
||||
// Moto-CAN
|
||||
#define MOTO_CAN_RX 26
|
||||
#define MOTO_CAN_TX 27
|
||||
/////
|
||||
#define WEIGHT_SCALE 276 //这是缩放值，根据砝码实测516.f
|
||||
#define WEIGHT_SCALE 276 // 这是缩放值，根据砝码实测516.f
|
||||
#define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms
|
||||
enum HookStatus
|
||||
{
|
||||
HS_UnInit, //还未初始化
|
||||
HS_Down, // 货物下放中
|
||||
HS_DownSlow, //货物慢速下放中 --接近地面慢速，不受设置速度影响
|
||||
HS_WaitUnhook, //等待脱钩
|
||||
HS_Up, // 回收中
|
||||
HS_InStore, // 入仓中 --慢速，不受设置速度影响
|
||||
HS_Locked, //已到顶部锁定
|
||||
HS_Stop //已停止
|
||||
HS_UnInit, // 还未初始化
|
||||
HS_Down, // 货物下放中
|
||||
HS_DownSlow, // 货物慢速下放中 --接近地面慢速，不受设置速度影响
|
||||
HS_WaitUnhook, // 等待脱钩
|
||||
HS_Up, // 回收中
|
||||
HS_InStore, // 入仓中 --慢速，不受设置速度影响
|
||||
HS_Locked, // 已到顶部锁定
|
||||
HS_Stop // 已停止
|
||||
};
|
||||
|
||||
|
||||
13
src/main.cpp
13
src/main.cpp
@ -10,7 +10,6 @@
|
||||
#include <FastLED.h>
|
||||
#include <ESP32Servo.h>
|
||||
|
||||
|
||||
// 角度传感器
|
||||
// 收放线电机控制
|
||||
// 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚
|
||||
@ -110,7 +109,7 @@ void setup()
|
||||
|
||||
// 初始化RGB LED 显示黄色灯表示需要注意 LED
|
||||
FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
|
||||
if (!motocontrol.init(&myservo)) // 初始化电机控制
|
||||
if (!motocontrol.init(&myservo)) // 初始化电机控制
|
||||
printf("motocontrol init fault\n");
|
||||
|
||||
// 发送状态任务开启--500ms一次
|
||||
@ -133,17 +132,15 @@ 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);
|
||||
// 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() ;
|
||||
// HookStatus hs=motocontrol.gethooktatus() ;
|
||||
// control_status_t cs=motocontrol.getcontrolstatus() ;
|
||||
|
||||
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
|
||||
|
||||
|
||||
}
|
||||
// 初始化过程--
|
||||
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
|
||||
|
||||
@ -55,8 +55,8 @@ void moto::settime_acc(float acctime)
|
||||
_acctime = abs((_ds * SACCEL_TIME) / ((float)OUTPUT_MAX * MOTO_REDUCTION)); // 加减速时间
|
||||
else
|
||||
_acctime = acctime;
|
||||
|
||||
printf("settime_acc:%.2f\n",_acctime);
|
||||
|
||||
printf("settime_acc:%.2f\n", _acctime);
|
||||
}
|
||||
|
||||
void moto::setspeedtarget(float new_target)
|
||||
|
||||
30
src/moto.h
30
src/moto.h
@ -2,14 +2,13 @@
|
||||
#include <Arduino.h>
|
||||
#include <CAN.h>
|
||||
#include <ESP32SJA1000.h>
|
||||
#define MOTO_REVERSED 1 //正反转1为正转，-1为反转
|
||||
#define MOTO_REVERSED 1 // 正反转1为正转，-1为反转
|
||||
#define MOTO_REDUCTION 36
|
||||
#define MOTO_MAXANG 8192 //0-8191
|
||||
#define MOTO_MAXANG 8192 // 0-8191
|
||||
#define MOTO_MAXANG_HALF MOTO_MAXANG / 2
|
||||
|
||||
#define OUTPUT_MAX 400 //最大输出转速rpm
|
||||
#define SACCEL_TIME 100 //使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就 (200*SACCEL_TIME)/OUTPUT_MAX 秒
|
||||
|
||||
#define OUTPUT_MAX 400 // 最大输出转速rpm
|
||||
#define SACCEL_TIME 100 // 使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就 (200*SACCEL_TIME)/OUTPUT_MAX 秒
|
||||
|
||||
typedef struct _PID_TypeDef
|
||||
{
|
||||
@ -52,11 +51,10 @@ typedef struct
|
||||
|
||||
unsigned long starttime;
|
||||
|
||||
float output_speed_rpm; //输出轴速度 ---rpm
|
||||
float output_round_cnt; //输出轴 总圈数
|
||||
float output_speed_rpm; // 输出轴速度 ---rpm
|
||||
float output_round_cnt; // 输出轴 总圈数
|
||||
} moto_measure_t;
|
||||
|
||||
|
||||
/// PID控制器
|
||||
void pid_param_init(PID_TypeDef *pid,
|
||||
uint16_t maxout,
|
||||
@ -69,29 +67,29 @@ void pid_param_init(PID_TypeDef *pid,
|
||||
float kd);
|
||||
inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd);
|
||||
inline void pid_target(PID_TypeDef *pid, float new_target);
|
||||
float pid_cal(PID_TypeDef *pid, int16_t measure,float pidtarget);
|
||||
float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget);
|
||||
void pid_init();
|
||||
|
||||
class moto
|
||||
{
|
||||
private:
|
||||
bool _closed;
|
||||
float _ds;
|
||||
float _ds;
|
||||
int16_t _start_speed;
|
||||
float _acctime;
|
||||
void set_moto_current(int16_t iq1);
|
||||
static void get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr);
|
||||
static void onReceive(int packetSize);
|
||||
|
||||
public:
|
||||
moto(); // 构造函数
|
||||
~moto(); // 析构函数
|
||||
bool init();
|
||||
void update();
|
||||
//设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0)
|
||||
//否则setspeedtarget(0)会按照settime_acc的时间停止
|
||||
void setspeedtarget(float new_target);
|
||||
void settime_acc(float acctime=-1); //设置加速和减速时间
|
||||
void close(); //关闭电机供电
|
||||
// 设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0)
|
||||
// 否则setspeedtarget(0)会按照settime_acc的时间停止
|
||||
void setspeedtarget(float new_target);
|
||||
void settime_acc(float acctime = -1); // 设置加速和减速时间
|
||||
void close(); // 关闭电机供电
|
||||
moto_measure_t getmotoinfo();
|
||||
|
||||
};
|
||||
|
||||
@ -168,7 +168,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
goodsstartup();
|
||||
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))
|
||||
{
|
||||
_hooksstatus = HS_DownSlow;
|
||||
_moto_dji.settime_acc(500);
|
||||
@ -176,7 +176,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
//开始慢速下放
|
||||
// 开始慢速下放
|
||||
case HookStatus::HS_DownSlow:
|
||||
{
|
||||
if (!_controlstatus.is_havegoods)
|
||||
@ -186,7 +186,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
//等待脱钩
|
||||
// 等待脱钩
|
||||
case HookStatus::HS_WaitUnhook:
|
||||
{
|
||||
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
|
||||
@ -199,7 +199,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
//开始收回
|
||||
// 开始收回
|
||||
case HookStatus::HS_Up:
|
||||
{
|
||||
// 会自动设置HS_Up状态
|
||||
@ -229,7 +229,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
|
||||
{
|
||||
//入仓时会自动设置速度，原速度不可用
|
||||
// 入仓时会自动设置速度，原速度不可用
|
||||
if (!_controlstatus.is_instore)
|
||||
_moto_dji.setspeedtarget(_runspeed);
|
||||
_servotatus = SS_ServoUnLocked;
|
||||
@ -300,12 +300,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
|
||||
stop();
|
||||
// 开始入仓
|
||||
//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;
|
||||
if (_controlstatus.is_havegoods)
|
||||
instlen=INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm)*INSTORE_LENGTH_SPEED;
|
||||
else
|
||||
instlen=INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm)*INSTORE_LENGTH_SPEED;
|
||||
instlen = INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
|
||||
else
|
||||
instlen = INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
|
||||
if ((_curr_length < instlen) && (_controlstatus.motostatus == MotoStatus::MS_Up))
|
||||
{
|
||||
if (!_controlstatus.is_instore)
|
||||
@ -414,16 +414,17 @@ void Motocontrol::moto_goodsdown(float length)
|
||||
_autogoodsdown = true;
|
||||
// 长度=0，直接中慢速下降，
|
||||
if (length == 0.0)
|
||||
|
||||
{ _goods_down_target_cnt =0;
|
||||
|
||||
{
|
||||
_goods_down_target_cnt = 0;
|
||||
setspeed(SPEED_HOOK_CHECK);
|
||||
_goods_speed = SPEED_HOOK_CHECK;
|
||||
}
|
||||
else
|
||||
{
|
||||
{
|
||||
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
|
||||
_goods_down_target_cnt = _goods_down_start_cnt + length / WHEEL_PERIMETER;
|
||||
setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
|
||||
setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
|
||||
_goods_speed = SPEED_HOOK_FAST;
|
||||
}
|
||||
moto_run(MS_Down);
|
||||
|
||||
@ -4,91 +4,79 @@
|
||||
#include "moto.h"
|
||||
#include <ESP32Servo.h>
|
||||
|
||||
#define ROPE_MAXLENGTH 700 //最多能放700cm---实际绳子应该比这个长750之类的
|
||||
#define WHEEL_DIAMETER 3.8 //轮子直径cm
|
||||
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长
|
||||
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数
|
||||
#define ROPE_MAXLENGTH 700 // 最多能放700cm---实际绳子应该比这个长750之类的
|
||||
#define WHEEL_DIAMETER 3.8 // 轮子直径cm
|
||||
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
|
||||
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
|
||||
|
||||
#define INSTORE_LENGTH_MIN_NONE 10.0f //最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
#define INSTORE_LENGTH_MIN_GOODS 8.0f //最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
#define INSTORE_LENGTH_MIN_NONE 10.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
#define INSTORE_LENGTH_MIN_GOODS 8.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||||
|
||||
#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
|
||||
|
||||
#define SPEED_MAX 350 // 最快收放线速度--任何时候不应该超过这个速度
|
||||
#define SPEED_INSTORE 5 // 入仓速度,自动重量归零也是这个速度下降2cm
|
||||
#define SPEED_BTN_SLOW 20 // 按键慢收放线速度(没有设置顶点时只能按这个速度收线)
|
||||
#define SPEED_BTN_MID 100 // 按键中等收放线速度
|
||||
#define SPEED_BTN_FAST 200 // 按键最快收放线速度
|
||||
#define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_SLOW 20 // 货物快到地面速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_CHECK 50 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
|
||||
#define SPEED_HOOK_UP SPEED_MAX // 空钩上升速度
|
||||
#define SPEED_UNBLOCK 0.1 // 舵机堵转需要缓慢转的速度
|
||||
|
||||
#define TM_UNBLOCK 100 // 舵机堵转需要缓慢转等待的时间ms
|
||||
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
|
||||
#define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
|
||||
|
||||
#define SPEED_MAX 350 //最快收放线速度--任何时候不应该超过这个速度
|
||||
#define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm
|
||||
#define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
|
||||
#define SPEED_BTN_MID 100 //按键中等收放线速度
|
||||
#define SPEED_BTN_FAST 200 //按键最快收放线速度
|
||||
#define SPEED_HOOK_FAST SPEED_MAX //货物下放速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下
|
||||
#define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
|
||||
#define SPEED_HOOK_UP SPEED_MAX //空钩上升速度
|
||||
#define SPEED_UNBLOCK 0.1 //舵机堵转需要缓慢转的速度
|
||||
#define SERVO_LOCKPOS 1890 // 舵机锁定位置
|
||||
#define SERVO_UNLOCKPOS 1780 // 舵机解锁位置
|
||||
#define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
|
||||
|
||||
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
|
||||
|
||||
#define HOOK_WEIHT_MAX 6000 // 最大货物重量 大于这个认为超重不工作 (克)
|
||||
|
||||
#define TM_UNBLOCK 100 //舵机堵转需要缓慢转等待的时间ms
|
||||
#define TM_SERVOLOCK 300 //舵机转到Lock和Unlock等待的时间ms
|
||||
#define TM_ACC_HS 500 //货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
|
||||
|
||||
|
||||
|
||||
#define SERVO_LOCKPOS 1890 //舵机锁定位置
|
||||
#define SERVO_UNLOCKPOS 1780 //舵机解锁位置
|
||||
#define SERVO_BLOCKUNLOCKPOS 1700 //舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
|
||||
|
||||
#define HOOK_WEIHT_MIN 100 //最小货物重量 小于这个认为没挂东西 (克)
|
||||
|
||||
#define HOOK_WEIHT_MAX 6000 //最大货物重量 大于这个认为超重不工作 (克)
|
||||
|
||||
#define HOOK_SLOWDOWN_LENGTH 10 //下放物体时快到目标长度时慢速长度 (cm)
|
||||
#define HOOK_UNHOOK_TIME 1500 //等待脱钩时间ms
|
||||
#define HOOK_SLOWDOWN_LENGTH 10 // 下放物体时快到目标长度时慢速长度 (cm)
|
||||
#define HOOK_UNHOOK_TIME 1500 // 等待脱钩时间ms
|
||||
|
||||
// 挂钩状态
|
||||
enum MotoStatus
|
||||
{
|
||||
MS_Down, // 自动下放中
|
||||
MS_Up, // 回收中
|
||||
MS_Stop //停止中
|
||||
MS_Down, // 自动下放中
|
||||
MS_Up, // 回收中
|
||||
MS_Stop // 停止中
|
||||
};
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool is_instore; //正在进仓
|
||||
bool is_setzero; //已设置0长度位置
|
||||
bool is_toplocked; //已到顶锁住
|
||||
bool is_overweight; //是否超重
|
||||
bool is_havegoods; //是否有货物
|
||||
float zero_cnt; //0长度位置
|
||||
float speed_targe; //当前目标速度
|
||||
MotoStatus motostatus; //电机运行状态
|
||||
bool is_instore; // 正在进仓
|
||||
bool is_setzero; // 已设置0长度位置
|
||||
bool is_toplocked; // 已到顶锁住
|
||||
bool is_overweight; // 是否超重
|
||||
bool is_havegoods; // 是否有货物
|
||||
float zero_cnt; // 0长度位置
|
||||
float speed_targe; // 当前目标速度
|
||||
MotoStatus motostatus; // 电机运行状态
|
||||
|
||||
} control_status_t;
|
||||
// 锁定舵机状态
|
||||
enum ServoStatus
|
||||
{
|
||||
SS_WaitMotoStop, //等待电机停止
|
||||
|
||||
SS_ServoUnLocked, //
|
||||
SS_WaitMotoStop, // 等待电机停止
|
||||
|
||||
SS_WaitUnLock, //
|
||||
SS_WaitServo, //等待舵机不堵转
|
||||
SS_WaitUnBlock, //等待舵机不堵转的位置--同时电机会缓慢旋转
|
||||
SS_ServoLocked //成功停止
|
||||
SS_ServoUnLocked, //
|
||||
|
||||
SS_WaitUnLock, //
|
||||
SS_WaitServo, // 等待舵机不堵转
|
||||
SS_WaitUnBlock, // 等待舵机不堵转的位置--同时电机会缓慢旋转
|
||||
SS_ServoLocked // 成功停止
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
class Motocontrol
|
||||
{
|
||||
private:
|
||||
Servo* _lockservo;
|
||||
Servo *_lockservo;
|
||||
moto _moto_dji;
|
||||
float _start_cnt;
|
||||
float _target_cnt;
|
||||
@ -99,7 +87,7 @@ private:
|
||||
|
||||
control_status_t _controlstatus;
|
||||
bool _check_targetlength;
|
||||
ServoStatus _servotatus;
|
||||
ServoStatus _servotatus;
|
||||
unsigned long _tm_servotatus;
|
||||
int _pullweight;
|
||||
HookStatus _hooksstatus;
|
||||
@ -112,33 +100,34 @@ private:
|
||||
|
||||
uint8_t _notweightcount;
|
||||
uint8_t _unblocktimes;
|
||||
unsigned long _tm_waitunhook;
|
||||
unsigned long _tm_waitunhook;
|
||||
float _runspeed;
|
||||
|
||||
void checkmotostatus();
|
||||
void checkhookstatus();
|
||||
void goodsstartup() ;
|
||||
void goodsstartup();
|
||||
void checkgoods();
|
||||
bool checkservoblock();
|
||||
void lockservo() ;
|
||||
void unlockservo() ;
|
||||
void lockservo();
|
||||
void unlockservo();
|
||||
void set_hook_speed(float hooksspeed);
|
||||
|
||||
public:
|
||||
Motocontrol(); // 构造函数
|
||||
~Motocontrol(); // 析构函数
|
||||
void setspeed(float motospeed,float acctime=-1); // 设置速度
|
||||
void stop(float acctime=0); // 停止
|
||||
void setzero(); // 设置0长度位置
|
||||
int16_t getlength(); // 得到长度
|
||||
void update(); // 更新
|
||||
bool init(Servo* lockservo); // 初始化
|
||||
void direct_speed(float motospeed); //直接设置电机速度--立马生效,仅用于测试
|
||||
moto_measure_t getmotoinfo(); //得到电机信息
|
||||
control_status_t getcontrolstatus(); //得到控制信息
|
||||
void setlocked(bool locked); //是否到顶锁定
|
||||
void moto_run(MotoStatus updown,float length= 0.0f) ;// 收放线
|
||||
void setweight(int pullweight);//设置货物实时重量
|
||||
void moto_goodsdown(float length= 0.0f);// 放下货物,自动回收
|
||||
Motocontrol(); // 构造函数
|
||||
~Motocontrol(); // 析构函数
|
||||
void setspeed(float motospeed, float acctime = -1); // 设置速度
|
||||
void stop(float acctime = 0); // 停止
|
||||
void setzero(); // 设置0长度位置
|
||||
int16_t getlength(); // 得到长度
|
||||
void update(); // 更新
|
||||
bool init(Servo *lockservo); // 初始化
|
||||
void direct_speed(float motospeed); // 直接设置电机速度--立马生效,仅用于测试
|
||||
moto_measure_t getmotoinfo(); // 得到电机信息
|
||||
control_status_t getcontrolstatus(); // 得到控制信息
|
||||
void setlocked(bool locked); // 是否到顶锁定
|
||||
void moto_run(MotoStatus updown, float length = 0.0f); // 收放线
|
||||
void setweight(int pullweight); // 设置货物实时重量
|
||||
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
|
||||
void moto_goodsdownresume(); // 恢复自动放线
|
||||
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
|
||||
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
|
||||
|
||||
Loading…
Reference in New Issue
Block a user