最近在学习webots这个软件,又因为Robocon赛事上舵轮是一大热点,所以想利用webots软件做一下舵轮的简单仿真
舵轮
之前做了四轮小车的模型,对于四个舵轮轮系来看,就是加了四个转向轮,每个轮系由一个驱动轮和一个转向轮构成。
分析并建立模型
- 在robot节点下,建立一个车体(body)节点,作为底盘
- 创建一个HingeJoint作为第一个轮系的转向轮,以此为父节点,再建立一个Hingejoint作为驱动轮。
- 调整各个节点的坐标,旋转轴等各个参数
- 复制第一个轮系,形成底盘,如图
- 图中,蓝色小方块仅用来区分车头
创建控制器
#include <webots/robot.h>
#include <webots/motor.h>
#define TIME_STEP 64
#define num 3.1415926f/180.0f
float angle;
int main(int argc, char **argv) {
wb_robot_init();
WbDeviceTag lf_dir_motor
= wb_robot_get_device("lf_dir_motor");
WbDeviceTag lf_val_motor
= wb_robot_get_device("lf_val_motor");
wb_motor_set_position(lf_val_motor,INFINITY);
wb_motor_set_velocity(lf_val_motor,0);
WbDeviceTag lb_dir_motor
= wb_robot_get_device("lb_dir_motor");
WbDeviceTag lb_val_motor
= wb_robot_get_device("lb_val_motor");
wb_motor_set_position(lb_val_motor,INFINITY);
wb_motor_set_velocity(lb_val_motor,0);
WbDeviceTag rf_dir_motor
= wb_robot_get_device("rf_dir_motor");
WbDeviceTag rf_val_motor
= wb_robot_get_device("rf_val_motor");
wb_motor_set_position(rf_val_motor,INFINITY);
wb_motor_set_velocity(rf_val_motor,0);
WbDeviceTag rb_dir_motor
= wb_robot_get_device("rb_dir_motor");
WbDeviceTag rb_val_motor
= wb_robot_get_device("rb_val_motor");
wb_motor_set_position(rb_val_motor,INFINITY);
wb_motor_set_velocity(rb_val_motor,0);
while (wb_robot_step(TIME_STEP) != -1) {
wb_motor_set_velocity(lf_val_motor,-20);
wb_motor_set_velocity(rf_val_motor,-20);
wb_motor_set_velocity(lb_val_motor,-20);
wb_motor_set_velocity(rb_val_motor,-20);
for(int i =0;i<360;i++)
{
angle = -i * num ;
wb_motor_set_position(lf_dir_motor,angle);
wb_motor_set_position(rf_dir_motor,angle);
wb_motor_set_position(lb_dir_motor,angle);
wb_motor_set_position(rb_dir_motor,angle);
wb_robot_step(1);
}
for(int i =360;i>0;i--)
{
angle = -i * num ;
wb_motor_set_position(lf_dir_motor,angle);
wb_motor_set_position(rf_dir_motor,angle);
wb_motor_set_position(lb_dir_motor,angle);
wb_motor_set_position(rb_dir_motor,angle);
wb_robot_step(1);
}
};
wb_robot_cleanup();
return 0;
}