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

增加电机缓启动和停止

Browse Source 
增加舵机锁定和解锁时和电机的配合，防止电机还在转就锁定和防止正在解锁电机开始高速旋转导致磨损
入仓长度随速度平方比例变化，防止速度太快刹不住车--
放货物变慢距离是否也需要这样还不确定，测试确定
增加速度，按实际330的速度收放货物，按钮双击和长按速度也加快
This commit is contained in:
pxzleo 2023-05-07 02:56:19 +08:00
parent 2f0e904107
commit ae901e70cc
5 changed files with 186 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/config.h
View File

@ -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

55
src/moto.cpp
View File

@ -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)

12
src/moto.h
View File

@ -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(); // 析构函数

121
src/motocontrol.cpp
View File

@ -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);
}

50
src/motocontrol.h
View File

@ -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(); // 析构函数