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

仅格式化代码，没有任何其他修改

Browse Source
This commit is contained in:
pxzleo 2023-05-12 00:13:59 +08:00
parent 5f414479f1
commit 93ec10391a
8 changed files with 152 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/Serialcommand.cpp
View File

@ -28,7 +28,8 @@ void Serialcommand::Uart_Command_Rev(void)
while (Serial2.available() > 0) while (Serial2.available() > 0)
{ {
ch = Serial2.read(); ch = Serial2.read();
if (DEBUG_OUT) printf("rev:%d\n", ch); if (DEBUG_OUT)
printf("rev:%d\n", ch);
switch (CmdState) switch (CmdState)
{ {
// 开始标志1 // 开始标志1
@ -37,7 +38,8 @@ void Serialcommand::Uart_Command_Rev(void)
if (HEAD_HI == ch) if (HEAD_HI == ch)
{ {
CmdState = 1; CmdState = 1;
if (DEBUG_OUT) printf("st:%d\n", CmdState); if (DEBUG_OUT)
printf("st:%d\n", CmdState);
} }
} }
break; break;
@ -48,7 +50,8 @@ void Serialcommand::Uart_Command_Rev(void)
{ {
bufferIndex = 0; bufferIndex = 0;
CmdState = 2; CmdState = 2;
if (DEBUG_OUT) printf("st:%d\n", CmdState); if (DEBUG_OUT)
printf("st:%d\n", CmdState);
} }
else else
CmdState = 0; CmdState = 0;
@ -58,13 +61,15 @@ void Serialcommand::Uart_Command_Rev(void)
case 2: case 2:
{ {
datalen = ch; datalen = ch;
if (DEBUG_OUT) printf("datalen:%d\n", datalen); if (DEBUG_OUT)
printf("datalen:%d\n", datalen);
if (datalen <= inbuffersize) if (datalen <= inbuffersize)
{ {
CmdState = 3; CmdState = 3;
bufferIndex = 0; bufferIndex = 0;
inBuffer[bufferIndex] = ch; 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 // 超长 else // 超长
{ {
@ -75,13 +80,15 @@ void Serialcommand::Uart_Command_Rev(void)
// 收数据直到结束 // 收数据直到结束
case 3: 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) if (bufferIndex <= datalen)
{ {
bufferIndex++; bufferIndex++;
inBuffer[bufferIndex] = ch; 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) // 收完了 if (bufferIndex == datalen) // 收完了
{ {
CmdState = 0; CmdState = 0;
@ -89,12 +96,14 @@ void Serialcommand::Uart_Command_Rev(void)
bufferIndex = 0; bufferIndex = 0;
return; // 不收数据了，先出去解析 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; CmdState = 0;
bufferIndex = 0; bufferIndex = 0;
recv_flag=false; recv_flag = false;
return; return;
} }
} }
@ -102,9 +111,9 @@ void Serialcommand::Uart_Command_Rev(void)
} }
} }
} }
//CRC8 // CRC8
//多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107） // 多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107）
//https://zhuanlan.zhihu.com/p/114049042 // https://zhuanlan.zhihu.com/p/114049042
uint8_t Serialcommand::getsum(const uint8_t *buffer, size_t size) 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);
@ -119,7 +128,7 @@ bool Serialcommand::checksum()
} }
return bufsum == chksum; return bufsum == chksum;
} }
void Serialcommand::sendinfo(HookStatus status,int linelength,int pullweight ) void Serialcommand::sendinfo(HookStatus status, int linelength, int pullweight)
{ {
// 长度 // 长度
@ -127,15 +136,14 @@ void Serialcommand::sendinfo(HookStatus status,int linelength,int pullweight )
outBuffer[3] = 0x02; outBuffer[3] = 0x02;
outBuffer[4] = status; outBuffer[4] = status;
outBuffer[5] =(uint8_t) ((linelength>>8) &0xff); outBuffer[5] = (uint8_t)((linelength >> 8) & 0xff);
outBuffer[6] = (uint8_t) (linelength&0xff); outBuffer[6] = (uint8_t)(linelength & 0xff);
outBuffer[7] =(uint8_t) ((pullweight>>8) &0xff); outBuffer[7] = (uint8_t)((pullweight >> 8) & 0xff);
outBuffer[8] = (uint8_t) (pullweight&0xff); outBuffer[8] = (uint8_t)(pullweight & 0xff);
outBuffer[9] = getsum(outBuffer + 2, 7); outBuffer[9] = getsum(outBuffer + 2, 7);
Serial2.write(outBuffer, 10); Serial2.write(outBuffer, 10);
} }
void Serialcommand::sendcmdret(uint8_t cmdret) void Serialcommand::sendcmdret(uint8_t cmdret)
@ -155,7 +163,8 @@ void Serialcommand::getcommand()
// 收到有效数据 // 收到有效数据
if (recv_flag) 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; recv_flag = false;
// 处理数据 // 处理数据
if (checksum()) if (checksum())

2
src/Serialcommand.h
View File

@ -22,5 +22,5 @@ public:
Serialcommand(); // 构造函数 Serialcommand(); // 构造函数
~Serialcommand(); // 析构函数 ~Serialcommand(); // 析构函数
void getcommand(); void getcommand();
void sendinfo(HookStatus status,int linelength,int pullweight ); void sendinfo(HookStatus status, int linelength, int pullweight);
}; };

21
src/config.h
View File

@ -1,10 +1,10 @@
#pragma once #pragma once
//////////// ////////////
//定义公共结构，变量，硬件接口等 // 定义公共结构，变量，硬件接口等
/// ///
// //
//硬件接口定义//////////////////////////// // 硬件接口定义////////////////////////////
// 按钮 // 按钮
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND #define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
#define BTN_DOWN 22 // 放线开关 #define BTN_DOWN 22 // 放线开关
@ -14,24 +14,23 @@
#define LOADCELL_DOUT_PIN 16 #define LOADCELL_DOUT_PIN 16
#define LOADCELL_SCK_PIN 17 #define LOADCELL_SCK_PIN 17
/////////////////////////////////////////// ///////////////////////////////////////////
#define SERVO_PIN 14 //锁定舵机PWM控制脚 #define SERVO_PIN 14 // 锁定舵机PWM控制脚
////LED ////LED
#define LED_DATA_PIN 25 #define LED_DATA_PIN 25
//Moto-CAN // Moto-CAN
#define MOTO_CAN_RX 26 #define MOTO_CAN_RX 26
#define MOTO_CAN_TX 27 #define MOTO_CAN_TX 27
///// /////
#define WEIGHT_SCALE 276 //这是缩放值，根据砝码实测516.f #define WEIGHT_SCALE 276 // 这是缩放值，根据砝码实测516.f
#define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms #define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms
enum HookStatus enum HookStatus
{ {
HS_UnInit, //还未初始化 HS_UnInit, // 还未初始化
HS_Down, // 货物下放中 HS_Down, // 货物下放中
HS_DownSlow, //货物慢速下放中 --接近地面慢速，不受设置速度影响 HS_DownSlow, // 货物慢速下放中 --接近地面慢速，不受设置速度影响
HS_WaitUnhook, //等待脱钩 HS_WaitUnhook, // 等待脱钩
HS_Up, // 回收中 HS_Up, // 回收中
HS_InStore, // 入仓中 --慢速，不受设置速度影响 HS_InStore, // 入仓中 --慢速，不受设置速度影响
HS_Locked, //已到顶部锁定 HS_Locked, // 已到顶部锁定
HS_Stop //已停止 HS_Stop // 已停止
}; };

5
src/main.cpp
View File

@ -10,7 +10,6 @@
#include <FastLED.h> #include <FastLED.h>
#include <ESP32Servo.h> #include <ESP32Servo.h>
// 角度传感器 // 角度传感器
// 收放线电机控制 // 收放线电机控制
// 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚 // 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚
@ -139,11 +138,9 @@ void showinfo()
// printf("PullWeight:%d\n", pullweight); // printf("PullWeight:%d\n", pullweight);
// HookStatus hs=motocontrol.gethooktatus() ; // HookStatus hs=motocontrol.gethooktatus() ;
//control_status_t cs=motocontrol.getcontrolstatus() ; // control_status_t cs=motocontrol.getcontrolstatus() ;
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus); // printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
} }
// 初始化过程-- // 初始化过程--
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成 // 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成

2
src/moto.cpp
View File

@ -56,7 +56,7 @@ void moto::settime_acc(float acctime)
else else
_acctime = acctime; _acctime = acctime;
printf("settime_acc:%.2f\n",_acctime); printf("settime_acc:%.2f\n", _acctime);
} }
void moto::setspeedtarget(float new_target) void moto::setspeedtarget(float new_target)

26
src/moto.h
View File

@ -2,14 +2,13 @@
#include <Arduino.h> #include <Arduino.h>
#include <CAN.h> #include <CAN.h>
#include <ESP32SJA1000.h> #include <ESP32SJA1000.h>
#define MOTO_REVERSED 1 //正反转1为正转，-1为反转 #define MOTO_REVERSED 1 // 正反转1为正转，-1为反转
#define MOTO_REDUCTION 36 #define MOTO_REDUCTION 36
#define MOTO_MAXANG 8192 //0-8191 #define MOTO_MAXANG 8192 // 0-8191
#define MOTO_MAXANG_HALF MOTO_MAXANG / 2 #define MOTO_MAXANG_HALF MOTO_MAXANG / 2
#define OUTPUT_MAX 400 //最大输出转速rpm #define OUTPUT_MAX 400 // 最大输出转速rpm
#define SACCEL_TIME 100 //使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就 (200*SACCEL_TIME)/OUTPUT_MAX 秒 #define SACCEL_TIME 100 // 使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就 (200*SACCEL_TIME)/OUTPUT_MAX 秒
typedef struct _PID_TypeDef typedef struct _PID_TypeDef
{ {
@ -52,11 +51,10 @@ typedef struct
unsigned long starttime; unsigned long starttime;
float output_speed_rpm; //输出轴速度 ---rpm float output_speed_rpm; // 输出轴速度 ---rpm
float output_round_cnt; //输出轴 总圈数 float output_round_cnt; // 输出轴 总圈数
} moto_measure_t; } moto_measure_t;
/// PID控制器 /// PID控制器
void pid_param_init(PID_TypeDef *pid, void pid_param_init(PID_TypeDef *pid,
uint16_t maxout, uint16_t maxout,
@ -69,7 +67,7 @@ void pid_param_init(PID_TypeDef *pid,
float kd); float kd);
inline void pid_reset(PID_TypeDef *pid, float kp, float ki, 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); 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(); void pid_init();
class moto class moto
@ -82,16 +80,16 @@ private:
void set_moto_current(int16_t iq1); void set_moto_current(int16_t iq1);
static void get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr); static void get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr);
static void onReceive(int packetSize); static void onReceive(int packetSize);
public: public:
moto(); // 构造函数 moto(); // 构造函数
~moto(); // 析构函数 ~moto(); // 析构函数
bool init(); bool init();
void update(); void update();
//设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0) // 设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0)
//否则setspeedtarget(0)会按照settime_acc的时间停止 // 否则setspeedtarget(0)会按照settime_acc的时间停止
void setspeedtarget(float new_target); void setspeedtarget(float new_target);
void settime_acc(float acctime=-1); //设置加速和减速时间 void settime_acc(float acctime = -1); // 设置加速和减速时间
void close(); //关闭电机供电 void close(); // 关闭电机供电
moto_measure_t getmotoinfo(); moto_measure_t getmotoinfo();
}; };

19
src/motocontrol.cpp
View File

@ -168,7 +168,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
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))
{ {
_hooksstatus = HS_DownSlow; _hooksstatus = HS_DownSlow;
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
@ -176,7 +176,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
} }
break; break;
} }
//开始慢速下放 // 开始慢速下放
case HookStatus::HS_DownSlow: case HookStatus::HS_DownSlow:
{ {
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
@ -186,7 +186,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
} }
break; break;
} }
//等待脱钩 // 等待脱钩
case HookStatus::HS_WaitUnhook: case HookStatus::HS_WaitUnhook:
{ {
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
@ -199,7 +199,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
} }
break; break;
} }
//开始收回 // 开始收回
case HookStatus::HS_Up: case HookStatus::HS_Up:
{ {
// 会自动设置HS_Up状态 // 会自动设置HS_Up状态
@ -229,7 +229,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if ((millis() - _tm_servotatus) > TM_SERVOLOCK) if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
{ {
//入仓时会自动设置速度，原速度不可用 // 入仓时会自动设置速度，原速度不可用
if (!_controlstatus.is_instore) if (!_controlstatus.is_instore)
_moto_dji.setspeedtarget(_runspeed); _moto_dji.setspeedtarget(_runspeed);
_servotatus = SS_ServoUnLocked; _servotatus = SS_ServoUnLocked;
@ -300,12 +300,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down)) if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
stop(); 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; float instlen;
if (_controlstatus.is_havegoods) if (_controlstatus.is_havegoods)
instlen=INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm)*INSTORE_LENGTH_SPEED; instlen = INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
else else
instlen=INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm)*INSTORE_LENGTH_SPEED; instlen = INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm) * INSTORE_LENGTH_SPEED;
if ((_curr_length < instlen) && (_controlstatus.motostatus == MotoStatus::MS_Up)) if ((_curr_length < instlen) && (_controlstatus.motostatus == MotoStatus::MS_Up))
{ {
if (!_controlstatus.is_instore) if (!_controlstatus.is_instore)
@ -415,7 +415,8 @@ void Motocontrol::moto_goodsdown(float length)
// 长度=0，直接中慢速下降， // 长度=0，直接中慢速下降，
if (length == 0.0) if (length == 0.0)
{ _goods_down_target_cnt =0; {
_goods_down_target_cnt = 0;
setspeed(SPEED_HOOK_CHECK); setspeed(SPEED_HOOK_CHECK);
_goods_speed = SPEED_HOOK_CHECK; _goods_speed = SPEED_HOOK_CHECK;
} }

119
src/motocontrol.h
View File

@ -4,91 +4,79 @@
#include "moto.h" #include "moto.h"
#include <ESP32Servo.h> #include <ESP32Servo.h>
#define ROPE_MAXLENGTH 700 //最多能放700cm---实际绳子应该比这个长750之类的 #define ROPE_MAXLENGTH 700 // 最多能放700cm---实际绳子应该比这个长750之类的
#define WHEEL_DIAMETER 3.8 //轮子直径cm #define WHEEL_DIAMETER 3.8 // 轮子直径cm
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长 #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
#define INSTORE_LENGTH_MIN_NONE 10.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_MIN_GOODS 8.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ #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 SERVO_LOCKPOS 1890 // 舵机锁定位置
#define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm #define SERVO_UNLOCKPOS 1780 // 舵机解锁位置
#define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线) #define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
#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 HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
#define HOOK_WEIHT_MAX 6000 // 最大货物重量 大于这个认为超重不工作 (克)
#define TM_UNBLOCK 100 //舵机堵转需要缓慢转等待的时间ms #define HOOK_SLOWDOWN_LENGTH 10 // 下放物体时快到目标长度时慢速长度 (cm)
#define TM_SERVOLOCK 300 //舵机转到Lock和Unlock等待的时间ms #define HOOK_UNHOOK_TIME 1500 // 等待脱钩时间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
// 挂钩状态 // 挂钩状态
enum MotoStatus enum MotoStatus
{ {
MS_Down, // 自动下放中 MS_Down, // 自动下放中
MS_Up, // 回收中 MS_Up, // 回收中
MS_Stop //停止中 MS_Stop // 停止中
}; };
typedef struct typedef struct
{ {
bool is_instore; //正在进仓 bool is_instore; // 正在进仓
bool is_setzero; //已设置0长度位置 bool is_setzero; // 已设置0长度位置
bool is_toplocked; //已到顶锁住 bool is_toplocked; // 已到顶锁住
bool is_overweight; //是否超重 bool is_overweight; // 是否超重
bool is_havegoods; //是否有货物 bool is_havegoods; // 是否有货物
float zero_cnt; //0长度位置 float zero_cnt; // 0长度位置
float speed_targe; //当前目标速度 float speed_targe; // 当前目标速度
MotoStatus motostatus; //电机运行状态 MotoStatus motostatus; // 电机运行状态
} control_status_t; } control_status_t;
// 锁定舵机状态 // 锁定舵机状态
enum ServoStatus enum ServoStatus
{ {
SS_WaitMotoStop, //等待电机停止 SS_WaitMotoStop, // 等待电机停止
SS_ServoUnLocked, // SS_ServoUnLocked, //
SS_WaitUnLock, // SS_WaitUnLock, //
SS_WaitServo, //等待舵机不堵转 SS_WaitServo, // 等待舵机不堵转
SS_WaitUnBlock, //等待舵机不堵转的位置--同时电机会缓慢旋转 SS_WaitUnBlock, // 等待舵机不堵转的位置--同时电机会缓慢旋转
SS_ServoLocked //成功停止 SS_ServoLocked // 成功停止
}; };
class Motocontrol class Motocontrol
{ {
private: private:
Servo* _lockservo; Servo *_lockservo;
moto _moto_dji; moto _moto_dji;
float _start_cnt; float _start_cnt;
float _target_cnt; float _target_cnt;
@ -117,28 +105,29 @@ private:
void checkmotostatus(); void checkmotostatus();
void checkhookstatus(); void checkhookstatus();
void goodsstartup() ; void goodsstartup();
void checkgoods(); void checkgoods();
bool checkservoblock(); bool checkservoblock();
void lockservo() ; void lockservo();
void unlockservo() ; void unlockservo();
void set_hook_speed(float hooksspeed); void set_hook_speed(float hooksspeed);
public: 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 stop(float acctime = 0); // 停止
void setzero(); // 设置0长度位置 void setzero(); // 设置0长度位置
int16_t getlength(); // 得到长度 int16_t getlength(); // 得到长度
void update(); // 更新 void update(); // 更新
bool init(Servo* lockservo); // 初始化 bool init(Servo *lockservo); // 初始化
void direct_speed(float motospeed); //直接设置电机速度--立马生效,仅用于测试 void direct_speed(float motospeed); // 直接设置电机速度--立马生效,仅用于测试
moto_measure_t getmotoinfo(); //得到电机信息 moto_measure_t getmotoinfo(); // 得到电机信息
control_status_t getcontrolstatus(); //得到控制信息 control_status_t getcontrolstatus(); // 得到控制信息
void setlocked(bool locked); //是否到顶锁定 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 setweight(int pullweight); // 设置货物实时重量
void moto_goodsdown(float length= 0.0f);// 放下货物,自动回收 void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
void moto_goodsdownresume(); // 恢复自动放线 void moto_goodsdownresume(); // 恢复自动放线
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown