Webots学习笔记（四）---舵轮模型

最近在学习webots这个软件，又因为Robocon赛事上舵轮是一大热点，所以想利用webots软件做一下舵轮的简单仿真

舵轮

之前做了四轮小车的模型，对于四个舵轮轮系来看，就是加了四个转向轮，每个轮系由一个驱动轮和一个转向轮构成。

分析并建立模型

• 在robot节点下，建立一个车体（body）节点，作为底盘
• 创建一个HingeJoint作为第一个轮系的转向轮，以此为父节点，再建立一个Hingejoint作为驱动轮。
• 调整各个节点的坐标，旋转轴等各个参数
• 复制第一个轮系，形成底盘，如图
• 图中，蓝色小方块仅用来区分车头

创建控制器

#include 
#include 

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

运行仿真