From 93ec10391a53fa9ffacb46350b002f77fe2712d6 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Fri, 12 May 2023 00:13:59 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=85=E6=A0=BC=E5=BC=8F=E5=8C=96=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=EF=BC=8C=E6=B2=A1=E6=9C=89=E4=BB=BB=E4=BD=95=E5=85=B6?= =?UTF-8?q?=E4=BB=96=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Serialcommand.cpp | 55 +++++++++------- src/Serialcommand.h | 2 +- src/config.h | 37 ++++++----- src/main.cpp | 13 ++-- src/moto.cpp | 4 +- src/moto.h | 30 +++++---- src/motocontrol.cpp | 27 ++++---- src/motocontrol.h | 143 +++++++++++++++++++----------------------- 8 files changed, 152 insertions(+), 159 deletions(-) diff --git a/src/Serialcommand.cpp b/src/Serialcommand.cpp index a0993aa..2fabe88 100644 --- a/src/Serialcommand.cpp +++ b/src/Serialcommand.cpp @@ -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()) diff --git a/src/Serialcommand.h b/src/Serialcommand.h index cf25f8e..fb61bc3 100644 --- a/src/Serialcommand.h +++ b/src/Serialcommand.h @@ -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); }; diff --git a/src/config.h b/src/config.h index bd0d2f8..4936c93 100644 --- a/src/config.h +++ b/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 // 已停止 }; - diff --git a/src/main.cpp b/src/main.cpp index da5aeaf..8ae9475 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,7 +10,6 @@ #include #include - // 角度传感器 // 收放线电机控制 // 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚 @@ -110,7 +109,7 @@ void setup() // 初始化RGB LED 显示黄色灯表示需要注意 LED FastLED.addLeds(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开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成 diff --git a/src/moto.cpp b/src/moto.cpp index dd43178..a1fd4d7 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -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) diff --git a/src/moto.h b/src/moto.h index 62874b4..ec511e4 100644 --- a/src/moto.h +++ b/src/moto.h @@ -2,14 +2,13 @@ #include #include #include -#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(); - }; diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 52a5d69..ddc2f84 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -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); diff --git a/src/motocontrol.h b/src/motocontrol.h index 69ab4bf..6ae1bd5 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -4,91 +4,79 @@ #include "moto.h" #include -#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