智能小车项目（七）通过PID实现给定和实际速度值计算PWM输出

我们先看大脑（上位机nano）
在这里插入图片描述
keybord_ctrl节点发布’cmd_vel’消息消息类型为Twist队列大小为1

pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

if not stop: pub.publish(twist)

driver_node订阅这个消息 当有消息时cmd_vel_callback回掉函数处理消息调用set_car_motion处理消息

 rospy.init_node("driver_node", anonymous=False) 

    def cmd_vel_callback(self, msg):# 小车运动控制，订阅者回调函数# Car motion control, subscriber callback functionif not isinstance(msg, Twist): return# 下发线速度和角速度# Issue linear vel and angular velvx = msg.linear.xvy = msg.linear.yangular = msg.angular.z# 小车运动控制,vel: ±1, angular: ±5# Trolley motion control,vel=[-1, 1], angular=[-5, 5]# rospy.loginfo("cmd_velx: {}, cmd_vely: {}, cmd_ang: {}".format(vx, vy, angular))self.car.set_car_motion(vx, vy, angular)

    def set_car_motion(self, v_x, v_y, v_z):try:vx_parms = bytearray(struct.pack('h', int(v_x*1000)))vy_parms = bytearray(struct.pack('h', int(v_y*1000)))vz_parms = bytearray(struct.pack('h', int(v_z*1000)))cmd = [self.__HEAD, self.__DEVICE_ID, 0x00, self.FUNC_MOTION, self.__CAR_TYPE, \vx_parms[0], vx_parms[1], vy_parms[0], vy_parms[1], vz_parms[0], vz_parms[1]]cmd[2] = len(cmd) - 1checksum = sum(cmd, self.__COMPLEMENT) & 0xffcmd.append(checksum)self.ser.write(cmd)if self.__debug:print("motion:", cmd)time.sleep(self.__delay_time)except:print('---set_car_motion error!---')pass

再看小脑（下位机32）
下位机接到串口消息进行速度处理Motion_Ctrl

	case FUNC_MOTION:{uint8_t parm = (uint8_t) *(data_buf + 4);int16_t Vx_recv = *(data_buf + 6) << 8 | *(data_buf + 5);int16_t Vy_recv = *(data_buf + 8) << 8 | *(data_buf + 7);int16_t Vz_recv = *(data_buf + 10) << 8 | *(data_buf + 9);uint8_t adjust = parm & 0x80;DEBUG("motion: 0x%02X, %d, %d, %d\n", parm, Vx_recv, Vy_recv, Vz_recv);if (Vx_recv == 0 && Vy_recv == 0 && Vz_recv == 0){Motion_Stop(STOP_BRAKE);}else{Motion_Ctrl(Vx_recv, Vy_recv, Vz_recv, (adjust==0?0:1));}break;}

通过公式计算每个轮子的速度调用Mecanum_Ctrl函数处理速度

// 控制小车运动
void Motion_Ctrl(int16_t V_x, int16_t V_y, int16_t V_z, uint8_t adjust)
{switch (g_car_type){case CAR_MECANUM:{Mecanum_Ctrl(V_x, V_y, V_z, adjust);break;}case CAR_MECANUM_MAX:{if (V_x > CAR_X3_PLUS_MAX_SPEED)  V_x = CAR_X3_PLUS_MAX_SPEED;if (V_x < -CAR_X3_PLUS_MAX_SPEED) V_x = -CAR_X3_PLUS_MAX_SPEED;if (V_y > CAR_X3_PLUS_MAX_SPEED)  V_y = CAR_X3_PLUS_MAX_SPEED;if (V_y < -CAR_X3_PLUS_MAX_SPEED) V_y = -CAR_X3_PLUS_MAX_SPEED;Mecanum_Ctrl(V_x, V_y, V_z, adjust);break;}case CAR_FOURWHEEL:{Fourwheel_Ctrl(V_x, V_y, V_z, adjust);break;}case CAR_ACKERMAN:{Ackerman_Ctrl(V_x, V_y, V_z, adjust);break;}default:break;}
}

调用Motion_Set_Speed设置给定速度到motor_data结构体中

typedef struct _motor_data_t
{float speed_mm_s[4];        // 输入值，编码器计算速度float speed_pwm[4];         // 输出值，PID计算出PWM值int16_t speed_set[4];       // 速度设置值
} motor_data_t;

void Motion_Set_Speed(int16_t speed_m1, int16_t speed_m2, int16_t speed_m3, int16_t speed_m4)
{g_start_ctrl = 1;motor_data.speed_set[0] = speed_m1;motor_data.speed_set[1] = speed_m2;motor_data.speed_set[2] = speed_m3;motor_data.speed_set[3] = speed_m4;for (uint8_t i = 0; i < MAX_MOTOR; i++){PID_Set_Motor_Target(i, motor_data.speed_set[i]*1.0);}
}

将目标速度给入pid_motor[i].target_val中

typedef struct _pid
{float target_val;               //目标值float output_val;               //输出值float pwm_output;        		//PWM输出值float Kp,Ki,Kd;          		//定义比例、积分、微分系数float err;             			//定义偏差值float err_last;          		//定义上一个偏差值float err_next;                 //定义下一个偏差值, 增量式float integral;          		//定义积分值，位置式
} pid_t;
void PID_Set_Motor_Target(uint8_t motor_id, float target)
{if (motor_id > MAX_MOTOR) return;if (motor_id == MAX_MOTOR){for (int i = 0; i < MAX_MOTOR; i++){pid_motor[i].target_val = target;}}else{pid_motor[motor_id].target_val = target;}
}

到这我们完成给定输入

然后我们看实际和给定之间的计算

void Motion_Handle(void)
{Motion_Get_Speed(&car_data);if (g_start_ctrl){Motion_Set_Pwm(motor_data.speed_pwm[0], motor_data.speed_pwm[1], motor_data.speed_pwm[2], motor_data.speed_pwm[3]);}
}

Motion_Get_Speed(&car_data);

首先得到编码器的当前值，编码器的值如何传过来的后面再说
通过该值计算当前各轮速度
通过各轮速度由公式计算当前小车xyz速度

 
// 从编码器读取当前各轮子速度，单位mm/s
void Motion_Get_Speed(car_data_t* car)
{int i = 0;float speed_mm[MAX_MOTOR] = {0};float circle_mm = Motion_Get_Circle_MM();float circle_pulse = Motion_Get_Circle_Pulse();float robot_APB = Motion_Get_APB();Motion_Get_Encoder();// 计算轮子速度，单位mm/s。for (i = 0; i < 4; i++){speed_mm[i] = (g_Encoder_All_Offset[i]) * 100 * circle_mm / circle_pulse;//printf("speed%d : %f",i,speed_mm[i]);}switch (g_car_type){case CAR_MECANUM:{car->Vx = (speed_mm[0] + speed_mm[1] + speed_mm[2] + speed_mm[3]) / 4;car->Vy = -(speed_mm[0] - speed_mm[1] - speed_mm[2] + speed_mm[3]) / 4;car->Vz = -(speed_mm[0] + speed_mm[1] - speed_mm[2] - speed_mm[3]) / 4.0f / robot_APB * 1000;break;}case CAR_MECANUM_MAX:{car->Vx = (speed_mm[0] + speed_mm[1] + speed_mm[2] + speed_mm[3]) / 4;car->Vy = -(speed_mm[0] - speed_mm[1] - speed_mm[2] + speed_mm[3]) / 4;car->Vz = -(speed_mm[0] + speed_mm[1] - speed_mm[2] - speed_mm[3]) / 4.0f / robot_APB * 1000;
//				printf("linear_x:%d\n",car->Vx);
//				printf("linear_y:%d\n",car->Vy);
//				printf("linear_z:%f\n",car->Vz);break;}case CAR_ACKERMAN:{car->Vx = (speed_mm[1] + speed_mm[3]) / 2;car->Vy = Ackerman_Get_Steer_Angle();car->Vz = -(speed_mm[1] - speed_mm[3]) * 1000 / robot_APB;break;}    default:break;}if (g_start_ctrl){for (i = 0; i < MAX_MOTOR; i++){motor_data.speed_mm_s[i] = speed_mm[i];}#if ENABLE_YAW_ADJUST//这个宏关的if (g_yaw_adjust){#if ENABLE_INV_MEMSMecanum_Yaw_Calc(ICM20948_Get_Yaw_Now());#elif ENABLE_MPU9250Mecanum_Yaw_Calc(MPU_Get_Yaw_Now());#endif}#endifPID_Calc_Motor(&motor_data);#if PID_ASSISTANT_EN//这个宏关的if (start_tool()){int32_t speed_send = car->Vx;// int32_t speed_send = (int32_t)speed_m1;set_computer_value(SEND_FACT_CMD, CURVES_CH1, &speed_send, 1);}#endif}
}

调用这个函数计算值
PID_Calc_Motor(&motor_data);

void PID_Calc_Motor(motor_data_t* motor)
{int i;// float pid_out[4] = {0};// for (i = 0; i < MAX_MOTOR; i++)// {//     pid_out[i] = PID_Location_Calc(&pid_motor[i], 0);//     PID_Set_Motor_Target(i, pid_out[i]);// }for (i = 0; i < MAX_MOTOR; i++){motor->speed_pwm[i] = PID_Incre_Calc(&pid_motor[i], motor->speed_mm_s[i]);}
}

调用PID_Incre_Calc这个函数完成PID计算计算出的值给到 motor->speed_pwm中

typedef struct _pid
{float target_val;               //目标值float output_val;               //输出值float pwm_output;        		//PWM输出值float Kp,Ki,Kd;          		//定义比例、积分、微分系数float err;             			//定义偏差值float err_last;          		//定义上两个个偏差值float err_next;                 //定义上一个偏差值, 增量式float integral;          		//定义积分值，位置式
} pid_t;

u(k)=Kp * e(k-1)+Ki *e(k) +Kd *(e(k)-2e(k-1)+e(k-2))+u(k-1)；

float PID_Incre_Calc(pid_t *pid, float actual_val)
{/*计算目标值与实际值的误差*/pid->err = pid->target_val - actual_val;/*PID算法实现*/pid->pwm_output += pid->Kp * (pid->err - pid->err_next) + pid->Ki * pid->err + pid->Kd * (pid->err - 2 * pid->err_next + pid->err_last);/*传递误差*/pid->err_last = pid->err_next;pid->err_next = pid->err;/*返回PWM输出值 这里如果忽略死区将MOTOR_IGNORE_PULSE设置为0*/if (pid->pwm_output > (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE))pid->pwm_output = (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE);if (pid->pwm_output < (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE))pid->pwm_output = (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE);return pid->pwm_output;// // 计算偏差// pid->err = pid->target_val - actual_val;// //增量式PI控制器// pid->pwm_output += pid->Kp * (pid->err - pid->err_last) + pid->Ki * pid->err;// if (pid->pwm_output > (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE))//     pid->pwm_output = (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE);// if (pid->pwm_output < (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE))//     pid->pwm_output = (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE);// pid->err_last = pid->err;// return pid->pwm_output;
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/619379.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

感染嗜肺军团菌是什么感觉？

感染嗜肺军团菌是什么感觉？

记录一下最近生病的一次经历吧&#xff0c;可能加我好友的朋友注意到了&#xff0c;前几天我发了个圈&#xff0c;有热心的朋友还专门私信了我说明了他自己的情况和治疗经验&#xff0c;感谢他们。 ​ 那么关于这次生病的经历&#xff0c;给大家分享一下。 首先&#xff0c;这次…
阅读更多...
【Linux】网络版计算器代码

【Linux】网络版计算器代码

网络应用层协议的demo 运输层基于TCP protocol.hpp #pragma once#include <iostream> #include <cstring> #include <string> #include <sys/types.h> #include <sys/socket.h> #include <jsoncpp/json/json.h>#define SEP " "…
阅读更多...
redis夯实之路-持久化之RDB与AOF详解

redis夯实之路-持久化之RDB与AOF详解

数据库 初始化服务器时会根据redisServer的dbnum属性来决定创建多少个数据库&#xff0c;默认为16 使用select切换数据库 客服端状态redisClient结构的db属性记录了当前的目标数据库 RedisDb结构的dict字典保存了数据库的所有键值对&#xff0c;这个字典被称为键空间。 cru…
阅读更多...
C++I/O流——(2)预定义格式的输入/输出(第二节)

C++I/O流——(2)预定义格式的输入/输出(第二节)

归纳编程学习的感悟&#xff0c; 记录奋斗路上的点滴&#xff0c; 希望能帮到一样刻苦的你&#xff01; 如有不足欢迎指正&#xff01; 共同学习交流&#xff01; &#x1f30e;欢迎各位→点赞 &#x1f44d; 收藏⭐ 留言​&#x1f4dd; 含泪播种的人一定能含笑收获&#xff…
阅读更多...
【MySQL】MySQL表的约束-空属性/默认值/列属性/zerofill/主键/自增长/唯一键/外键

【MySQL】MySQL表的约束-空属性/默认值/列属性/zerofill/主键/自增长/唯一键/外键

文章目录 表的约束1.空属性 --null && not null2.默认值 -- default3.列描述4.zerofill5.主键6.自增长7.唯一键8.外键 表的约束 表的约束&#xff1a;表中一定要有各种约束&#xff0c;通过约束&#xff0c;让我们未来插入数据库表中的数据是符合预期的。约束的本质是…
阅读更多...
桶排序（Java语言）

桶排序（Java语言）

视频讲解地址&#xff1a;【手把手带你写十大排序】8.桶排序&#xff08;Java语言&#xff09;_哔哩哔哩_bilibili 代码&#xff1a; public class BucketSort {public void sortFunction(int[] array, int bucketNum) {int max Integer.MIN_VALUE, min Integer.MAX_VALUE;…
阅读更多...
【QT】多层QTreeWidget与QStackedWidget的关联操作

【QT】多层QTreeWidget与QStackedWidget的关联操作

通过点击多层QTreeWidget来控制QStackedWidget中的page页面切换 treeWidget设计 treeWidget设计&#xff1a; // treeWidget设计ui->treeWidget->clear();ui->treeWidget->setColumnCount(1);//第一层QStringList l;l<<"管理系统";QTreeWid…
阅读更多...
JavaWeb-HTTP

JavaWeb-HTTP

一、概念 HTTP&#xff1a;HyperText Transfer Protocol&#xff0c;超文本传输协议。读者应该不是第一次接触这个名词&#xff0c;但可能仍然不是很理解&#xff0c;笔者将逐一解释。 HyperText&#xff08;超文本&#xff09;&#xff1a;根据维基百科&#xff0c;Hypertex…
阅读更多...
Linux数据处理的几个命令

Linux数据处理的几个命令

文章目录 Linux数据处理的几个命令文件内容搜索利器 - grep语法默认无参数增加文件夹反向查找不区分大小写显示行数&#xff0c;精准定位 没有规矩不成方圆 sort语法默认无参数根据第N列排序检查是否已经排序逆序排列 你是唯一的 uniq语法默认无参数统计出现频次仅仅显示重复的…
阅读更多...
Go语言的调度器

Go语言的调度器

简介 Go语言的调度器是一个非常强大的工具&#xff0c;它可以帮助我们轻松地实现并发编程。调度器的工作原理是将多个协程映射到多个操作系统线程上&#xff0c;并根据协程的状态来决定哪个协程应该在哪个线程上运行。 调度器有两种主要策略&#xff1a; 协作式调度&#xf…
阅读更多...
入门Linux简单操作

入门Linux简单操作

基本命令 scp ✓ scp -r 文件 127.0.0.1&#xff1a;/root/文件 (source->>>>destination) mv cp ✓ cp xxxx ./xxxx date ✓ 修改时间 date -s “yyyy-MM-dd 12:12:59” find ✓ find /home/user -name “*.txt” grep ✓ 管道 软连接 多用户 免密设置 脚…
阅读更多...
iPhone“查找”最多可添加32个物品！

iPhone“查找”最多可添加32个物品！

对于那些丢三落四的果粉来说&#xff0c;苹果的“查找”功能是一大福音。不管是丢失了iPhone、iPad、Mac、AirPods还是AirTag&#xff0c;都可以通过“查找”功能在地图上追踪设备的位置&#xff0c;甚至是远程锁定或抹掉设备的数据。 那么&#xff0c;iPhone的查找一次能支持添…
阅读更多...
LeetCode 38 外观数列

LeetCode 38 外观数列

题目描述 外观数列 给定一个正整数 n &#xff0c;输出外观数列的第 n 项。 「外观数列」是一个整数序列&#xff0c;从数字 1 开始&#xff0c;序列中的每一项都是对前一项的描述。 你可以将其视作是由递归公式定义的数字字符串序列&#xff1a; countAndSay(1) "1…
阅读更多...
软件测试|解决Github port 443 : Timed out连接超时的问题

软件测试|解决Github port 443 : Timed out连接超时的问题

前言 GitHub是全球最大的开源代码托管平台之一&#xff0c;许多开发者和团队使用它来管理和协作开源项目。但在当下&#xff0c;我们在clone或者提交代码时会经常遇到"GitHub Port 443: Timed Out"错误&#xff0c;这意味着我们的电脑无法建立与GitHub服务器的安全连…
阅读更多...
学习python仅此一篇就够了（函数进阶：lambda函数）

学习python仅此一篇就够了（函数进阶：lambda函数）

函数进阶 函数的多返回值 def ceshi1():return 1, 2x, y ceshi1() print(x) #1 print(y) #2 函数的多种传参方式 1.位置参数&#xff1a;调用函数时根据函数定义的参数位置来传递参数 def ceshi1(name,age):print(f"你的姓名是{name},你的年龄是{age}")ceshi1(…
阅读更多...
xtu oj 1522 格子

xtu oj 1522 格子

题目描述 一个nm的网格&#xff0c;格子里最多能放一枚棋子&#xff0c;将k枚棋子随机放入不同的网格中&#xff0c;使得同行同列最多只有一枚棋子&#xff0c;请问概率是多少&#xff1f; 输入格式 第一行是一个整数T (1≤T≤512)&#xff0c;表示样例的个数。 以后每行一…
阅读更多...
UniRepLKNet实战：使用UniRepLKNet实现图像分类任务（一）

UniRepLKNet实战：使用UniRepLKNet实现图像分类任务（一）

文章目录 摘要安装包安装timm 数据增强Cutout和MixupEMA项目结构计算mean和std生成数据集一些问题 摘要 大核卷积神经网络&#xff08;ConvNets&#xff09;近年来受到广泛关注&#xff0c;但仍存在两个关键问题需要进一步研究。首先&#xff0c;目前的大型卷积神经网络架构大…
阅读更多...
设计模式之避免共享的设计模式Copy-on-Write模式

设计模式之避免共享的设计模式Copy-on-Write模式

系列文章目录 设计模式之避免共享的设计模式Immutability&#xff08;不变性&#xff09;模式 设计模式之并发特定场景下的设计模式 Two-phase Termination&#xff08;两阶段终止&#xff09;模式 文章目录 系列文章目录一、Copy-on-Write是什么&#xff1f;二、应用 一、Cop…
阅读更多...
士兵队列 +队列queue+模拟

士兵队列 +队列queue+模拟

士兵队列 队列queue 某部队进行新兵队列训练&#xff0c;将新兵从一开始按顺序依次编号&#xff0c;并排成一行横队&#xff0c;训练的规则如下&#xff1a;从头开始一至二报数&#xff0c;凡报到二的出列&#xff0c;剩下的向小序号方向靠拢&#xff0c;再从头开始进行一至三…
阅读更多...
javacv和opencv对图文视频编辑-按指定间隔从视频抽取缩略图

javacv和opencv对图文视频编辑-按指定间隔从视频抽取缩略图

1、java代码抽取视频缩略图 直接上代码 首先pom 引入了javacv和Thumbnails <!-- 引入javacv --><dependency><groupId>org.bytedeco</groupId><artifactId>javacv</artifactId><version>1.5.6</version></dependency>…
阅读更多...
最新文章

Copyright @ 2022~2023