From ae901e70cc2513bc8edcc2543021b56c697e590b Mon Sep 17 00:00:00 2001 From: pxzleo Date: Sun, 7 May 2023 02:56:19 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=94=B5=E6=9C=BA=E7=BC=93?= =?UTF-8?q?=E5=90=AF=E5=8A=A8=E5=92=8C=E5=81=9C=E6=AD=A2=20=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E8=88=B5=E6=9C=BA=E9=94=81=E5=AE=9A=E5=92=8C=E8=A7=A3?= =?UTF-8?q?=E9=94=81=E6=97=B6=E5=92=8C=E7=94=B5=E6=9C=BA=E7=9A=84=E9=85=8D?= =?UTF-8?q?=E5=90=88=EF=BC=8C=E9=98=B2=E6=AD=A2=E7=94=B5=E6=9C=BA=E8=BF=98?= =?UTF-8?q?=E5=9C=A8=E8=BD=AC=E5=B0=B1=E9=94=81=E5=AE=9A=E5=92=8C=E9=98=B2?= =?UTF-8?q?=E6=AD=A2=E6=AD=A3=E5=9C=A8=E8=A7=A3=E9=94=81=E7=94=B5=E6=9C=BA?= =?UTF-8?q?=E5=BC=80=E5=A7=8B=E9=AB=98=E9=80=9F=E6=97=8B=E8=BD=AC=E5=AF=BC?= =?UTF-8?q?=E8=87=B4=E7=A3=A8=E6=8D=9F=20=E5=85=A5=E4=BB=93=E9=95=BF?= =?UTF-8?q?=E5=BA=A6=E9=9A=8F=E9=80=9F=E5=BA=A6=E5=B9=B3=E6=96=B9=E6=AF=94?= =?UTF-8?q?=E4=BE=8B=E5=8F=98=E5=8C=96=EF=BC=8C=E9=98=B2=E6=AD=A2=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E5=A4=AA=E5=BF=AB=E5=88=B9=E4=B8=8D=E4=BD=8F=E8=BD=A6?= =?UTF-8?q?--=20=E6=94=BE=E8=B4=A7=E7=89=A9=E5=8F=98=E6=85=A2=E8=B7=9D?= =?UTF-8?q?=E7=A6=BB=E6=98=AF=E5=90=A6=E4=B9=9F=E9=9C=80=E8=A6=81=E8=BF=99?= =?UTF-8?q?=E6=A0=B7=E8=BF=98=E4=B8=8D=E7=A1=AE=E5=AE=9A=EF=BC=8C=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E7=A1=AE=E5=AE=9A=20=E5=A2=9E=E5=8A=A0=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=EF=BC=8C=E6=8C=89=E5=AE=9E=E9=99=85330=E7=9A=84?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=E6=94=B6=E6=94=BE=E8=B4=A7=E7=89=A9=EF=BC=8C?= =?UTF-8?q?=E6=8C=89=E9=92=AE=E5=8F=8C=E5=87=BB=E5=92=8C=E9=95=BF=E6=8C=89?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=E4=B9=9F=E5=8A=A0=E5=BF=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/config.h | 4 +- src/moto.cpp | 55 ++++++++++---------- src/moto.h | 12 ++++- src/motocontrol.cpp | 121 ++++++++++++++++++++++++++++++++++++++------ src/motocontrol.h | 50 ++++++++++++++---- 5 files changed, 186 insertions(+), 56 deletions(-) diff --git a/src/config.h b/src/config.h index 0f243c4..bd0d2f8 100644 --- a/src/config.h +++ b/src/config.h @@ -14,10 +14,12 @@ #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 +//Moto-CAN +#define MOTO_CAN_RX 26 +#define MOTO_CAN_TX 27 ///// #define WEIGHT_SCALE 276 //这是缩放值，根据砝码实测516.f #define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间 ms diff --git a/src/moto.cpp b/src/moto.cpp index 906d7b8..961d354 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -1,10 +1,14 @@ #include "moto.h" #include "Arduino.h" +#include "config.h" + uint8_t CaninBuffer[8]; // 接收指令缓冲区 moto_measure_t moto_chassis; PID_TypeDef moto_pid; + + moto::moto() { _runing = false; @@ -15,7 +19,7 @@ bool moto::init() { Serial.println("init moto"); pid_init(); - CAN.setPins(26, 27); + CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); // start the CAN bus at 500 kbps if (!CAN.begin(1000E3)) { @@ -29,39 +33,31 @@ moto::~moto() { } -// int16_t delta; -// int16_t max_speed_change = 100; // 500 ok 100电源不报高压错误但有点震荡 -// int16_t set_speed_temp; + void moto::update() { - - // //可以柔和改变速度，柔和程度max_speed_change越小越柔和,但太小需要调PID - // for (uint8_t i = 0; i < 2; i++) - // { - // // 加减速 - // delta = (int16_t)moto_pid.target - moto_chassis.speed_rpm; - // if (delta > max_speed_change) - // set_speed_temp = (float)(moto_chassis.speed_rpm + max_speed_change); - // else if (delta < -max_speed_change) - // set_speed_temp = (float)(moto_chassis.speed_rpm - max_speed_change); - // else - // set_speed_temp = moto_pid.target; - // pid_cal(&moto_pid, moto_chassis.speed_rpm,set_speed_temp); - // } - if ((_runing || brake)&&!_closed) { - pid_cal(&moto_pid, moto_chassis.speed_rpm, moto_pid.target); + + + unsigned long dt=millis() - moto_chassis.starttime; //时间差 + float x=0.0f; + float mspeed=0.0f; + float _msoftspeed=moto_pid.target; + if (dt<_acctime) + { + x=float(dt)/_acctime; //归一化时间 + //公式参考 https://mp.weixin.qq.com/s/bhdvA3Ex6lAWVmBril-Sag + mspeed=(-2*x*x*x+3*x*x);//S曲线速度公式--计算归一化速度 + _msoftspeed=_start_speed+mspeed*_ds; + } + pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed); set_moto_current(moto_pid.output); + // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n", + // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output); } } -// void moto::update() -// { -// pid_cal(&moto_pid, moto_chassis.speed_rpm); -// set_moto_current(moto_pid.output); -// } - void moto::settarget(float new_target) { // 为了马上停止，跳过PID控制器 @@ -81,6 +77,13 @@ void moto::settarget(float new_target) } moto_pid.target = new_target * MOTO_REDUCTION; + + moto_chassis.starttime=millis(); // 换算为绝对时间 + _start_speed=moto_chassis.speed_rpm; + _ds=moto_pid.target-_start_speed; //速度差 + _acctime=abs((_ds*SACCEL_TIME)/((float)OUTPUT_MAX*MOTO_REDUCTION)); //加减速时间 + + printf("set_target:tm:%d,target:%.2f\n",moto_chassis.starttime,moto_pid.target); } void moto::set_moto_current(int16_t iq1) diff --git a/src/moto.h b/src/moto.h index 5660a49..53a2d38 100644 --- a/src/moto.h +++ b/src/moto.h @@ -7,6 +7,9 @@ #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 秒 + typedef struct _PID_TypeDef { @@ -47,11 +50,13 @@ typedef struct int32_t total_angle; uint32_t msg_cnt; + unsigned long starttime; + float output_speed_rpm; //输出轴速度 ---rpm float output_round_cnt; //输出轴 总圈数 - } moto_measure_t; + /// PID控制器 void pid_param_init(PID_TypeDef *pid, uint16_t maxout, @@ -72,10 +77,13 @@ class moto private: bool _runing; bool _closed; + 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(); // 析构函数 diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index c6ac8da..120db7c 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -15,7 +15,6 @@ Motocontrol::Motocontrol() _controlstatus.motostatus = MotoStatus::MS_Stop; _controlstatus.speed_targe = SPEED_BTN_SLOW; _check_targetlength = false; - _need_closemoto = false; _pullweight = 0; _curr_length = 0.0; _hooksstatus = HS_UnInit; @@ -23,6 +22,8 @@ Motocontrol::Motocontrol() _overweightcount = 0; _notweightcount = 0; _autogoodsdown = false; + _servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机 + _unblocktimes = 0; } Motocontrol::~Motocontrol() { @@ -31,6 +32,10 @@ Motocontrol::~Motocontrol() bool Motocontrol::init(Servo *lockservo) // 初始化 { _lockservo = lockservo; + // 不管堵转问题，马上会执行其他初始化让舵机介绍 + _lockservo->write(SERVO_LOCKPOS); + _servotatus = SS_ServoLocked; + return _moto_dji.init(); } void Motocontrol::setspeed(float motospeed) // 设置速度 @@ -81,16 +86,29 @@ void Motocontrol::checkgoods() // 检测是否超重 _notweightcount = 0; } } +void Motocontrol::lockservo() // 锁定舵机 +{ + printf("start_lockservo\n"); + _servotatus = SS_WaitMotoStop; + _tm_servotatus = millis(); +} +void Motocontrol::unlockservo() // 解锁舵机 +{ + printf("unlockservo\n"); + // 解锁操作 + _lockservo->write(SERVO_UNLOCKPOS); + _moto_dji.settarget(-1.0f);//解锁时逆向慢速转动 + _servotatus = SS_WaitUnLock; + _tm_servotatus = millis(); + _unblocktimes = 0; +} void Motocontrol::stop() // 停止 { + printf("stop \n"); _moto_dji.settarget(0.0f); _controlstatus.motostatus = MS_Stop; _hooksstatus = HS_Stop; - _lockservo->write(SERVO_LOCKPOS); - _need_closemoto = true; - _tm_closemoto = millis(); - printf("write servo :%d \n", SERVO_LOCKPOS); - printf("stop \n"); + lockservo(); } void Motocontrol::setlocked(bool locked) { @@ -209,13 +227,85 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 } } } +// 检测舵机堵转---电流大或者角度不够，需要传感器 +bool Motocontrol::checkservoblock() +{ + return false; +} void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候停 { - // 1秒后断电，防止电调堵转保护 - if ((_need_closemoto) && ((millis() - _tm_closemoto) > 1000)) - _moto_dji.close(); - moto_measure_t mf = _moto_dji.getmotoinfo(); + + // 停止状态---1防止舵机堵转，2延时关闭电机供电 + switch (_servotatus) + { + case SS_WaitUnLock: //舵机解锁 + { + if ((millis() - _tm_servotatus) > TM_SERVOLOCK) + { + _moto_dji.settarget(_runspeed); + _servotatus=SS_ServoUnLocked; + } + break; + } + + + + //等待电机停止 + case SS_WaitMotoStop: + { + if (abs(mf.speed_rpm)<3) + { + _lockservo->write(SERVO_LOCKPOS); + _servotatus=SS_WaitServo; + _tm_servotatus = millis(); + } + break; + } + // 等待舵机执行到位---------防止堵转，功能没有使用，采用硬件防堵转舵机 + case SS_WaitServo: + { + // 舵机执行时间 + if ((millis() - _tm_servotatus) > TM_SERVOLOCK) + { + // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 + if (!checkservoblock() || (_unblocktimes > 2)) + { + printf("SS not servoblock close moto \n"); + _moto_dji.close(); + _servotatus = SS_ServoLocked; + } + else + // 堵转 + { + printf("SS servoblock unlock servo and turn moto \n"); + _tm_servotatus = millis(); + _servotatus = SS_WaitUnBlock; + // 写一个不会堵转的位置 + _lockservo->write(SERVO_BLOCKUNLOCKPOS); + // 转一点电机角度 + _moto_dji.settarget(SPEED_UNBLOCK); + } + } + + break; + } + case SS_WaitUnBlock: + { + if ((millis() - _tm_servotatus) > TM_UNBLOCK) + { + printf("SS lock servo \n"); + // 继续锁定等待舵机检测 + _lockservo->write(SERVO_LOCKPOS); + _servotatus = SS_WaitServo; + _tm_servotatus = millis(); + _unblocktimes++; + } + break; + } + } + + // 已设置零点 if (_controlstatus.is_setzero) { @@ -225,7 +315,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 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_MIN+(mf.output_speed_rpm*mf.output_speed_rpm)*INSTORE_LENGTH_SPEED)) && (_controlstatus.motostatus == MotoStatus::MS_Up)) { _moto_dji.settarget(-SPEED_INSTORE); if (!_controlstatus.is_instore) @@ -298,7 +388,7 @@ void Motocontrol::moto_goodsdown(float length) return; } // 没挂东西 - if (!_controlstatus.is_havegoods ) + if (!_controlstatus.is_havegoods) { printf("goods min fault:%d \n", _pullweight); return; @@ -366,10 +456,8 @@ void Motocontrol::moto_run(MotoStatus updown, float length) else _hooksstatus = HS_Down; - _lockservo->write(SERVO_UNLOCKPOS); - _need_closemoto = false; + unlockservo(); - printf("write servo :%d \n", SERVO_UNLOCKPOS); _controlstatus.motostatus = updown; // 有长度限制 if (length != 0) @@ -398,5 +486,6 @@ void Motocontrol::moto_run(MotoStatus updown, float length) } // 开始转 printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt); - _moto_dji.settarget(runspeed); + _runspeed=runspeed; + // _moto_dji.settarget(runspeed); } \ No newline at end of file diff --git a/src/motocontrol.h b/src/motocontrol.h index 62913a7..ac4c65f 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -9,23 +9,33 @@ #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数 -#define INSTORE_LENGTH 10 //入仓长度,低于该长度要缓慢入仓 cm +#define INSTORE_LENGTH_MIN 8.0f //最小入仓长度,在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ +#define INSTORE_LENGTH_SPEED 0.0002f //最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ + + #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 100 //货物下放速度--有目标长度得情况下 +#define SPEED_BTN_MID 200 //按键中等收放线速度 +#define SPEED_BTN_FAST 330 //按键最快收放线速度 +#define SPEED_HOOK_FAST 330 //货物下放速度--有目标长度得情况下 #define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下 #define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度 -#define SPEED_HOOK_UP 50 //空钩上升速度 +#define SPEED_HOOK_UP 330 //空钩上升速度 +#define SPEED_UNBLOCK 0.1 //舵机堵转需要缓慢转的速度 -#define SERVO_LOCKPOS 1820 //舵机锁定位置 -#define SERVO_UNLOCKPOS 1600 //舵机解锁位置 +#define TM_UNBLOCK 100 //舵机堵转需要缓慢转等待的时间ms +#define TM_SERVOLOCK 200 //舵机转到Lock和Unlock等待的时间ms + + + + +#define SERVO_LOCKPOS 1890 //舵机锁定位置 +#define SERVO_UNLOCKPOS 1780 //舵机解锁位置 +#define SERVO_BLOCKUNLOCKPOS 1700 //舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 #define HOOK_WEIHT_MIN 100 //最小货物重量 小于这个认为没挂东西 (克) @@ -54,6 +64,20 @@ typedef struct MotoStatus motostatus; //电机运行状态 } control_status_t; +// 锁定舵机状态 +enum ServoStatus +{ + SS_WaitMotoStop, //等待电机停止 + + SS_ServoUnLocked, // + + SS_WaitUnLock, // + SS_WaitServo, //等待舵机不堵转 + SS_WaitUnBlock, //等待舵机不堵转的位置--同时电机会缓慢旋转 + SS_ServoLocked //成功停止 +}; + + @@ -68,8 +92,8 @@ private: float _target_cnt; control_status_t _controlstatus; bool _check_targetlength; - bool _need_closemoto; - unsigned long _tm_closemoto; + ServoStatus _servotatus; + unsigned long _tm_servotatus; int _pullweight; HookStatus _hooksstatus; int _hook_targetlen; @@ -80,13 +104,17 @@ private: uint8_t _overweightcount; uint8_t _notweightcount; - + uint8_t _unblocktimes; unsigned long _tm_waitunhook; + float _runspeed; void checkmotostatus(); void checkhookstatus(); void goodsstartup() ; void checkgoods(); + bool checkservoblock(); + void lockservo() ; + void unlockservo() ; public: Motocontrol(); // 构造函数 ~Motocontrol(); // 析构函数