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

效果如下
参数自己去算，自己去实验，还是挺直观的。