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

简化Moto类

Browse Source 
重构停止逻辑
This commit is contained in:
pxzleo 2023-05-09 22:41:01 +08:00
parent 6e9a21a2c4
commit c1dd4a6d49
5 changed files with 55 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main.cpp
View File

@ -493,5 +493,5 @@ void upbtn_pressend()
void testbtn_click()
{
Serial.println("testbtn_click");
motocontrol.moto_goodsdown(40);
motocontrol.moto_goodsdown(40); //二楼340 //桌子40
}

82
src/moto.cpp
View File

@ -2,18 +2,13 @@
#include "Arduino.h"
#include "config.h"
uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis;
PID_TypeDef moto_pid;
moto::moto()
{
_runing = false;
brake = true;
_closed=false;
_closed = true;
}
bool moto::init()
{
@ -33,62 +28,45 @@ moto::~moto()
{
}
void moto::update()
{
if ((_runing || brake)&&!_closed)
if (!_closed)
{
unsigned long dt=millis() - moto_chassis.starttime; //时间差
float x=0.0f;
float mspeed=0.0f;
float _msoftspeed=moto_pid.target;
if (dt<_acctime)
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;
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);
// 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::settime_acc(float acctime)
{
if (acctime==-1)
_acctime=abs((_ds*SACCEL_TIME)/((float)OUTPUT_MAX*MOTO_REDUCTION)); //加减速时间
else _acctime=acctime;
if (acctime == -1)
_acctime = abs((_ds * SACCEL_TIME) / ((float)OUTPUT_MAX * MOTO_REDUCTION)); // 加减速时间
else
_acctime = acctime;
printf("settime_acc:%.2f\n",_acctime);
}
void moto::settarget(float new_target)
void moto::setspeedtarget(float new_target)
{
// 为了马上停止，跳过PID控制器
if (new_target == 0.0f)
{
// 没刹车直接断开电流控制
_runing = false;
if (!brake)
{
pid_init();
set_moto_current(0);
}
}
else
{ _runing = true;
_closed=false;
}
moto_pid.target = new_target * MOTO_REDUCTION;
moto_chassis.starttime=millis(); // 换算为绝对时间
_start_speed=moto_chassis.speed_rpm;
_ds=moto_pid.target-_start_speed; //速度差
printf("set_target:tm:%d,target:%.2f\n",moto_chassis.starttime,moto_pid.target);
moto_chassis.starttime = millis(); // 换算为绝对时间
_start_speed = moto_chassis.speed_rpm;
_ds = moto_pid.target - _start_speed; // 速度差
_closed = false;
printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, moto_pid.target);
}
void moto::set_moto_current(int16_t iq1)
@ -119,9 +97,6 @@ void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr)
else if (ptr->angle - ptr->last_angle < -MOTO_MAXANG_HALF)
ptr->round_cnt++;
ptr->total_angle = ptr->round_cnt * MOTO_MAXANG + ptr->angle - ptr->offset_angle;
// ptr->output_speed_rpm= (float)ptr->speed_rpm / MOTO_REDUCTION;
// ptr->output_round_cnt= ptr->round_cnt + (float)(ptr->angle - ptr->offset_angle)/MOTO_MAXANG/ MOTO_REDUCTION;
}
moto_measure_t moto::getmotoinfo()
@ -131,11 +106,12 @@ moto_measure_t moto::getmotoinfo()
moto_chassis.output_round_cnt = (moto_chassis.round_cnt + (float)(moto_chassis.angle - moto_chassis.offset_angle) / MOTO_MAXANG) / MOTO_REDUCTION;
return moto_chassis;
}
//关闭电机输出，防止堵转
// 关闭电机输出，防止堵转
void moto::close()
{
set_moto_current(0);
_closed=true;
pid_init();
set_moto_current(0);
_closed = true;
}
void moto::onReceive(int packetSize)
{

12
src/moto.h
View File

@ -75,23 +75,23 @@ void pid_init();
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(); // 析构函数
bool brake; //刹车==默认true
bool init();
void update();
void settarget(float new_target);
void settime_acc(float acctime=-1);
//设置速度--如果需要立即停止需要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();
void close();
};

25
src/motocontrol.cpp
View File

@ -98,15 +98,16 @@ void Motocontrol::unlockservo() // 解锁舵机
printf("unlockservo\n");
// 解锁操作
_lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.settarget(-1.0f);//解锁时逆向慢速转动
_moto_dji.setspeedtarget(-1.0f);//解锁时逆向慢速转动
_servotatus = SS_WaitUnLock;
_tm_servotatus = millis();
_unblocktimes = 0;
}
void Motocontrol::stop() // 停止
void Motocontrol::stop(float acctime) // 停止
{
printf("stop \n");
_moto_dji.settarget(0.0f);
_moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop;
_hooksstatus = HS_Stop;
lockservo();
@ -244,7 +245,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{
if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
{
_moto_dji.settarget(_runspeed);
_moto_dji.setspeedtarget(_runspeed);
_servotatus=SS_ServoUnLocked;
}
break;
@ -285,7 +286,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 写一个不会堵转的位置
_lockservo->write(SERVO_BLOCKUNLOCKPOS);
// 转一点电机角度
_moto_dji.settarget(SPEED_UNBLOCK);
_moto_dji.setspeedtarget(SPEED_UNBLOCK);
}
}
@ -315,12 +316,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 不能超ROPE_MAXLENGTH
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))
{
_moto_dji.settarget(-SPEED_INSTORE);
if (!_controlstatus.is_instore)
{
_moto_dji.setspeedtarget(-SPEED_INSTORE);
_controlstatus.is_instore = true;
_hooksstatus = HS_InStore;
printf("begin instore \n");
@ -335,8 +336,8 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{
if (mf.output_round_cnt > _target_cnt)
{
printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop();
printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop(1000);//缓慢停止
}
}
else if (_controlstatus.motostatus == MS_Up)
@ -344,7 +345,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if (mf.output_round_cnt < _target_cnt)
{
printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop();
stop(1000);//缓慢停止
}
}
}
@ -358,7 +359,7 @@ void Motocontrol::update() // 更新
}
void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
{
_moto_dji.settarget(motospeed);
_moto_dji.setspeedtarget(motospeed);
}
moto_measure_t Motocontrol::getmotoinfo()
@ -488,5 +489,5 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 开始转
printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
_runspeed=runspeed;
// _moto_dji.settarget(runspeed);
// _moto_dji.setspeedtarget(runspeed);
}

12
src/motocontrol.h
View File

@ -4,7 +4,7 @@
#include "moto.h"
#include <ESP32Servo.h>
#define ROPE_MAXLENGTH 600 //最多能放600cm---实际绳子应该比这个长650之类的
#define ROPE_MAXLENGTH 700 //最多能放700cm---实际绳子应该比这个长750之类的
#define WHEEL_DIAMETER 4.0 //轮子直径cm
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数
@ -14,15 +14,15 @@
#define SPEED_MAX 200 //最快收放线速度--任何时候不应该超过这个速度
#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_FAST SPEED_MAX //按键最快收放线速度
#define SPEED_HOOK_FAST SPEED_MAX //货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP 100 //空钩上升速度
#define SPEED_HOOK_UP SPEED_MAX //空钩上升速度
#define SPEED_UNBLOCK 0.1 //舵机堵转需要缓慢转的速度
@ -119,7 +119,7 @@ public:
Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数
void setspeed(float motospeed,float acctime=-1); // 设置速度
void stop(); // 停止
void stop(float acctime=0); // 停止
void setzero(); // 设置0长度位置
int16_t getlength(); // 得到长度
void update(); // 更新