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;
}
效果如下
引數自己去算，自己去實驗，還是挺直觀的，
臨界阻尼計算公式：R=2√(L/C)
轉載請註明出處，本文鏈接：https://www.uj5u.com/ruanti/293658.html
標籤：其他