Webots 实现PD控制
- 世界搭建
- 代码
文件已分享到我的github，欢迎交流。
效果如下
欠阻尼状态
过阻尼状态如下
临界阻尼状态如下
下面开始展示搭建过程
世界搭建
新建一个空白的世界
。
。
右键空白处选择添加新的
。
。
添加背景光源
。
。
添加地板
。
。
添加后如下
。
。
添加一个机器人
。
。
机器人位置会出现一个坐标系
。
。
在机器人子类选择添加
。
。
为机器人主体添加一个形状
。
。
添加几何形状
。
。
选择为box形状
。
。
添加后出现一个白盒子，有一半在地里面
。
。
为方便观察将尺寸改大，并将其从地里面扯出来
。
。
在机器人子类上选择新建
。
。
添加一个直线滑轨
。
。
滑轨参数
。
。
在滑轨上添加一个直线电机
。
。
在滑轨上添加一个位置传感器
。
。
划归末端添加一个固体
。
。
给此固体添加形状
。
。
依旧选为box，尺寸也一样
。
。
改变颜色，方便观察
。
。
我改好后如下
。
。
将两个方块放到一起，方便观察
。
。
为滑轨末端的固体添加质量
。
。
设置质量和密度
先设置重量再关闭密度（设为-1），下面会报个错不用理他。
。
。
给电机和传感器起名字
在程序中会根据这个"name"去匹配相应的部件
。
。
改变滑轨初始位置，使得两个方块不挨在一起
至此仿真世界建立完毕
代码
新建controller
我用的c语言来编写这个控制器
我选择用vs来写代码，反正vs2019好用还免费。用第一个也行，都一样。
。。。。。。。。。。。。。。。。
。。。。。。。。。。。。。。。
。。。。。。。。。。。。。
。。。。。。。。。。。。
。。。。。。。。。。。
。。。。。。。。。。
。。。。。。。。。
。。。。。。。。
。。。。。。
。。。。。
。。。。
。。。
。。
。
程序如下
/*
* File: pid.c
* Date: 2021.08.10
* Description: PD Controller test
* Author: HiYoung
* Modifications: 2021.08.10
*/
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/position_sensor.h>
#define TIME_STEP 64
int main(int argc, char **argv)
{
wb_robot_init();
/*加载驱动*/
WbDeviceTag motor = wb_robot_get_device("linear motor1");//直线电机
WbDeviceTag sensor = wb_robot_get_device("position sensor1");//位置传感器
wb_position_sensor_enable(sensor, TIME_STEP);//位置传感器使能
double last_pos = 0;
/*临界阻尼*/
double kp = 100;
double kd = 301;
/*欠阻尼*/
//double kp = 100;
//double kd = 200;
/*过阻尼*/
//double kp = 100;
//double kd = 400;
while (wb_robot_step(TIME_STEP) != -1)
{
double pos = wb_position_sensor_get_value(sensor);
double force = kp * (0 - pos) + kd *(last_pos - pos);
/* F = Kp * x + Kd * dx */
last_pos = pos;
if (force > 500)
force = 500;
else if (force < -500)
force = -500;
wb_motor_set_force(motor, force);
printf("%f\r\n", pos);
};
wb_robot_cleanup();
return 0;
}效果如下
参数自己去算，自己去实验，还是挺直观的。
