From 15318bbe05009bae0605f52980ed2f7829b3b0d8 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Sat, 22 Apr 2023 16:59:32 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E9=94=81=E5=AE=9A=E8=88=B5?= =?UTF-8?q?=E6=9C=BA=EF=BC=8C=E7=94=B5=E6=9C=BA=E6=8E=A7=E5=88=B6=E8=B0=83?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- platformio.ini | 1 + src/config.h | 2 +- src/main.cpp | 42 ++++++++++++++++++---- src/moto.cpp | 86 +++++++++++++++++++++++++++++++++++++-------- src/moto.h | 7 +++- src/motocontrol.cpp | 29 +++++++++------ src/motocontrol.h | 16 ++++++--- 7 files changed, 143 insertions(+), 40 deletions(-) diff --git a/platformio.ini b/platformio.ini index ebdc672..e5d0e3d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -21,3 +21,4 @@ lib_deps = robtillaart/CRC@^0.3.3 sandeepmistry/CAN@^0.3.1 fastled/FastLED@^3.5.0 + madhephaestus/ESP32Servo@^0.13.0 diff --git a/src/config.h b/src/config.h index 8c9a432..ad4047e 100644 --- a/src/config.h +++ b/src/config.h @@ -16,7 +16,7 @@ #define LOADCELL_SCK_PIN 4 /////////////////////////////////////////// #define SPEED_UP_SLOW 4 //rmp 或者 弧度/秒 - +#define SERVO_PIN 14 //锁定舵机PWM控制脚 // 挂钩状态 diff --git a/src/main.cpp b/src/main.cpp index 7f3b297..eeb552e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,6 +8,8 @@ #include "motocontrol.h" #include "moto.h" #include +#include +#define TM_INSTORE_DELAY 100 //入仓延时关闭时间 ms // 角度传感器 // 收放线电机控制 @@ -19,6 +21,7 @@ OneButton button_down(BTN_DOWN, true); OneButton button_checktop(BTN_CT, true); HX711 scale; Serialcommand sercomm; +Servo myservo; #define NUM_LEDS 1 #define DATA_PIN 25 CRGB leds[NUM_LEDS]; @@ -50,6 +53,9 @@ void led_show(CRGB ledcolor); // void Task1code( void *pvParameters ); void showledidel(); int pullweight = 0; + unsigned long _tm_bengstop; + bool _bengstop=false; + void setup() { @@ -76,9 +82,17 @@ void setup() button_checktop.attachLongPressStart(ctbtn_pressstart); // 按下马上产生事件， button_checktop.attachLongPressStop(ctbtn_pressend); // 抬起 + ESP32PWM::allocateTimer(0); + ESP32PWM::allocateTimer(1); + ESP32PWM::allocateTimer(2); + ESP32PWM::allocateTimer(3); + myservo.setPeriodHertz(50); // standard 50 hz servo + myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object + + // 初始化RGB LED 显示黄色灯表示需要注意 LED FastLED.addLeds(leds, NUM_LEDS); // GRB ordering is typical - if (!motocontrol.init()) // 初始化电机控制 + if (!motocontrol.init(&myservo)) // 初始化电机控制 printf("motocontrol init fault\n"); // 发送状态任务开启--500ms一次 @@ -99,8 +113,8 @@ void slowup() } void showinfo() { - // moto_measure_t mf=motocontrol.getmotoinfo() ; - // printf("total_cnt:%.3f;speed:%.2f \n ", mf.output_round_cnt, mf.output_speed_rpm); + // 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); } @@ -116,6 +130,13 @@ void loop() motocontrol.update(); // 电机控制 showledidel(); //显示LED灯光 showinfo(); // 显示一些调试用信息 + if ((_bengstop)&&((millis()-_tm_bengstop)>TM_INSTORE_DELAY)) + { + _bengstop=false; + motocontrol.setlocked(true); + hookstatus = HS_Lock; + } + delay(1); } // 在核心0上执行耗时长的低优先级的 @@ -171,7 +192,11 @@ void showledidel() } case MotoStatus::MS_Up: { - led_show(255,0,0); // red + + if (motocontrol.getcontrolstatus().is_instore) + led_show(255,0,0); // red + else + led_show(200,0,50); // red break; } @@ -183,7 +208,7 @@ void showledidel() }else { if (motocontrol.getcontrolstatus().is_setzero) - led_show(255,0,255); // 紫色 + led_show(255,255,255); // 紫色 else led_show(255,255,0); //yellow } @@ -197,14 +222,17 @@ void showledidel() void ctbtn_pressstart() { Serial.println("ctbtn_pressstart"); - motocontrol.setlocked(true); - hookstatus = HS_Lock; + + _tm_bengstop=millis(); + _bengstop=true; + } void ctbtn_pressend() { Serial.println("ctbtn_pressend"); motocontrol.setlocked(false); + _bengstop=false; } void downbtn_click() { diff --git a/src/moto.cpp b/src/moto.cpp index b43c31b..906d7b8 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -7,11 +7,14 @@ PID_TypeDef moto_pid; moto::moto() { + _runing = false; + brake = true; + _closed=false; } bool moto::init() { Serial.println("init moto"); - pid_param_init(&moto_pid, 16000, 1000, 10, 8000, 0, 3.0f, 0.2f, 0.0f); + pid_init(); CAN.setPins(26, 27); // start the CAN bus at 500 kbps if (!CAN.begin(1000E3)) @@ -25,20 +28,64 @@ bool moto::init() moto::~moto() { } + +// int16_t delta; +// int16_t max_speed_change = 100; // 500 ok 100电源不报高压错误但有点震荡 +// int16_t set_speed_temp; void moto::update() { - pid_cal(&moto_pid, moto_chassis.speed_rpm); - set_moto_current(moto_pid.output); + + // //可以柔和改变速度，柔和程度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); + set_moto_current(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) { - moto_pid.target = new_target*MOTO_REDUCTION; + // 为了马上停止，跳过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; } void moto::set_moto_current(int16_t iq1) { - iq1=MOTO_REVERSED*iq1; + iq1 = MOTO_REVERSED * iq1; CAN.beginPacket(0x200, 8); CAN.write((uint8_t)((iq1 >> 8) & 0xFF)); CAN.write((uint8_t)(iq1 & 0xFF)); @@ -54,13 +101,10 @@ void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr) { // 处理数据 ptr->last_angle = ptr->angle; - ptr->angle = (uint16_t)MOTO_REVERSED*(message_ptr[0] << 8 | message_ptr[1]); // 角度 - ptr->speed_rpm = (int16_t)MOTO_REVERSED*(message_ptr[2] << 8 | message_ptr[3]); // 速度 - ptr->real_current = (int16_t)MOTO_REVERSED*(message_ptr[4] << 8 | message_ptr[5]); // 实际转矩电流 - ptr->temperature = message_ptr[6]; // 电机温度，2006没有，为0 - - - + ptr->angle = (uint16_t)MOTO_REVERSED * (message_ptr[0] << 8 | message_ptr[1]); // 角度 + ptr->speed_rpm = (int16_t)MOTO_REVERSED * (message_ptr[2] << 8 | message_ptr[3]); // 速度 + ptr->real_current = (int16_t)MOTO_REVERSED * (message_ptr[4] << 8 | message_ptr[5]); // 实际转矩电流 + ptr->temperature = message_ptr[6]; // 电机温度，2006没有，为0 if (ptr->angle - ptr->last_angle > MOTO_MAXANG_HALF) ptr->round_cnt--; @@ -74,11 +118,17 @@ void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr) moto_measure_t moto::getmotoinfo() { - //在中断里面计算时间太长会导致重启,在取信息时计算浮点 + // 在中断里面计算时间太长会导致重启,在取信息时计算浮点 moto_chassis.output_speed_rpm = (float)moto_chassis.speed_rpm / MOTO_REDUCTION; 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; +} void moto::onReceive(int packetSize) { if (!CAN.packetRtr()) @@ -91,6 +141,11 @@ void moto::onReceive(int packetSize) } } /////////////PID控制 +void pid_init() +{ + pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 10.0f, 1.0f, 0.5f); // + // pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 14.0f, 0.01f, 0.0f); //// +} void pid_param_init(PID_TypeDef *pid, uint16_t maxout, uint16_t intergral_limit, @@ -129,7 +184,7 @@ inline void pid_target(PID_TypeDef *pid, float new_target) pid->target = new_target; } -float pid_cal(PID_TypeDef *pid, int16_t measure) +float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget) { pid->lasttime = pid->thistime; @@ -137,7 +192,8 @@ float pid_cal(PID_TypeDef *pid, int16_t measure) pid->dtime = pid->thistime - pid->lasttime; // 更新两次计算的时间 pid->last_err = pid->err; - pid->err = pid->target - measure; + pid->err = pidtarget - measure; + // pid->err = pid->target - measure; if (abs(pid->err) > pid->DeadBand) { diff --git a/src/moto.h b/src/moto.h index 3ae2882..5660a49 100644 --- a/src/moto.h +++ b/src/moto.h @@ -64,11 +64,14 @@ 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 pid_cal(PID_TypeDef *pid, int16_t measure,float pidtarget); +void pid_init(); class moto { private: + bool _runing; + bool _closed; 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); @@ -76,8 +79,10 @@ private: public: moto(); // 构造函数 ~moto(); // 析构函数 + bool brake; //刹车==默认true bool init(); void update(); void settarget(float new_target); moto_measure_t getmotoinfo(); + void close(); }; diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 2b5256b..d0612aa 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -2,6 +2,7 @@ #include "Arduino.h" #include "motocontrol.h" #include "moto.h" +#include #define DEBUG_OUT false Motocontrol::Motocontrol() @@ -12,33 +13,30 @@ Motocontrol::Motocontrol() _controlstatus.motostatus = MotoStatus::MS_Stop; _controlstatus.speed_targe = SPEED_SLOW; _check_targetlength = false; + _need_closemoto=false; } Motocontrol::~Motocontrol() { } -bool Motocontrol::init() // 初始化 +bool Motocontrol::init(Servo *lockservo) // 初始化 { + _lockservo=lockservo; return _moto_dji.init(); } void Motocontrol::setspeed(float motospeed) // 设置速度 { _controlstatus.speed_targe = motospeed; } -void Motocontrol::down(float length) // 放线 -{ - printf("down:%.2f \n", length); - moto_run(MotoStatus::MS_Down, length); -} -void Motocontrol::up(float length) // 收线 -{ - printf("up:%.2f \n", length); - moto_run(MotoStatus::MS_Up, length); -} + void Motocontrol::stop() // 停止 { _moto_dji.settarget(0.0f); _controlstatus.motostatus = MS_Stop; + _lockservo->write(SERVO_LOCKPOS); + _need_closemoto=true; + _tm_closemoto=millis(); + printf("write servo :%d \n",SERVO_LOCKPOS); printf("stop \n"); } void Motocontrol::setlocked(bool locked) @@ -62,6 +60,11 @@ int16_t Motocontrol::getlength() // 得到长度 } void Motocontrol::checkstatus() // 检查状态，比如什么时候停 { + //1秒后断电 + if ((_need_closemoto)&&((millis()-_tm_closemoto)>1000)) + _moto_dji.close(); + + moto_measure_t mf = _moto_dji.getmotoinfo(); // 已设置零点 if (_controlstatus.is_setzero) @@ -136,6 +139,10 @@ void Motocontrol::moto_run(MotoStatus updown, float length) if (_controlstatus.speed_targe == 0) return; + _lockservo->write(SERVO_UNLOCKPOS); + _need_closemoto=false; + + printf("write servo :%d \n",SERVO_UNLOCKPOS); _controlstatus.motostatus = updown; // 有长度限制 if (length != 0) diff --git a/src/motocontrol.h b/src/motocontrol.h index be63d14..7676ac6 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -2,18 +2,21 @@ #include "Arduino.h" #include "config.h" #include "moto.h" +#include #define ROPE_MAXLENGTH 600 //最多能放600cm---实际绳子应该比这个长650之类的 #define WHEEL_DIAMETER 4.0 //轮子直径cm -#define INSTORE_LENGTH 15 //入仓长度,低于该长度要缓慢入仓 cm +#define INSTORE_LENGTH 10 //入仓长度,低于该长度要缓慢入仓 cm #define SPEED_MAX 330 //最快收放线速度 -#define SPEED_MIN 3 //入仓速度 +#define SPEED_MIN 5 //入仓速度 #define SPEED_SLOW 20 //慢收放线速度 #define SPEED_FAST 100 //快收放线速度 #define SPEED_BTN_FAST 200 //最快收放线速度 #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数 +#define SERVO_LOCKPOS 1900 //舵机锁定位置 +#define SERVO_UNLOCKPOS 1500 //舵机解锁位置 @@ -22,6 +25,7 @@ enum MotoStatus { MS_Down, // 自动下放中 MS_Up, // 回收中 + MS_Stop //停止中 }; @@ -43,28 +47,30 @@ typedef struct class Motocontrol { private: + Servo* _lockservo; moto _moto_dji; float _start_cnt; float _target_cnt; control_status_t _controlstatus; bool _check_targetlength; + bool _need_closemoto; + unsigned long _tm_closemoto; void checkstatus(); public: Motocontrol(); // 构造函数 ~Motocontrol(); // 析构函数 void setspeed(float motospeed); // 设置速度 - void down(float length); // 收线 - void up(float length); // 放线 void stop(); // 停止 void setzero(); // 设置0长度位置 int16_t getlength(); // 得到长度 void update(); // 更新 - bool init(); // 初始化 + 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) ;// 放线 + };