webots自学笔记(三)控制器与电机控制

时间:2024-03-10 20:14:18

      原创文章,来自“博客园,_阿龙clliu” http://www.cnblogs.com/clliu/,转载请注明原文章出处。

 

      上一次建了四足机器人的模型,模型文件在上一篇有下载地址,这一次用控制器让它走起来。由于在忙一些毕业设计的事情,以后的每一次的篇幅可能会短一点。Webots的编程语言支持C、C++、matlab、python、java,本文以C编写机器人控制器。

       在webots菜单栏:向导 -> 新机器人控制器 ,新建一个控制器,选择C语言,命名为my_controller。

      在每个servo节点下定义name属性,在控制器中要使用到。

      将下方代码代替控制器编辑文本my_controller的内容,点击编辑按钮,没有编译错误后再点击生成按钮 ,如果没有问题会出现是否恢复模拟,点击确定。(PS:这个过程会重置你的场景树中的模型,控制器和场景树两个部分最好是分别操作,完成后保存。)

 

/*
 * File:          my_robot.c
 * Date:          
 * Description:   
 * Author:        clliu
 * Modifications: 
 */


#include <webots/robot.h>
#include <webots/servo.h>
#include <assert.h>
#include <math.h>
#include <stdio.h>


#define TIME_STEP 16
#define rad_2_deg(X) ( X / pi * 180.0 )
#define deg_2_rad(X) ( X / 180.0 * pi )
#define pi 3.1415926
#define T 1


int main(int argc, char **argv)
{
  //定义webots电机设备
  WbDeviceTag servos[8];
  
  //定义电机名字,与场景树种servo节点下name属性对应
  const char *SERVO_NAMES[] = { 
   "fore_left_1",
   "fore_right_1", 
   "hind_left_1", 
   "hind_right_1", 
   "fore_left_2",
   "fore_right_2", 
   "hind_left_2", 
   "hind_right_2", 
    NULL };
    
  //CPG控制摆动参数  
  double Ah = 10 ;
  double Ak = 15 ;
  double t = 0.0;
  
  //机器人重置,webots内部函数
  wb_robot_init();
  printf("robot inited!\n");


  //将电机设备逐一加载
  int i;
  for (i = 0; SERVO_NAMES[i]; i++) {
    servos[i] = wb_robot_get_device(SERVO_NAMES[i]);
    assert(servos[i]);
  }


  //进入仿真执行的循环
  while (wb_robot_step(TIME_STEP) != -1) {
    


    //计算每个servo的角度,这个控制理论是基于CPG(中枢神经发生器)的,有兴趣可以自行了解
    double lf_hip_pos = Ah * sin(2 * pi / T * t - pi / 2);
    double lf_knee_pos = Ak * sin(2 * pi / T * t + pi) ;
    double lb_hip_pos =  Ah * sin(2 * pi / T * t + pi / 2);
    double lb_knee_pos = Ak * sin(2 * pi / T * t) ;
    double rf_hip_pos = Ah * sin(2 * pi / T * t + pi / 2);
    double rf_knee_pos = Ak * sin(2 * pi / T * t) ;
    double rb_hip_pos =  Ah * sin(2 * pi / T * t - pi / 2);
    double rb_knee_pos = Ak * sin(2 * pi / T * t + pi) ;
    

    //使用位置控制模式控制电机
     wb_servo_set_position(servos[0], deg_2_rad(lf_hip_pos));
     wb_servo_set_position(servos[1], deg_2_rad(rf_hip_pos));
     wb_servo_set_position(servos[2], deg_2_rad(lb_hip_pos));
     wb_servo_set_position(servos[3], deg_2_rad(rb_hip_pos));

     wb_servo_set_position(servos[4], deg_2_rad(lf_knee_pos));
     wb_servo_set_position(servos[5], deg_2_rad(rf_knee_pos));
     wb_servo_set_position(servos[6], deg_2_rad(lb_knee_pos));
     wb_servo_set_position(servos[7], deg_2_rad(rb_knee_pos));
     
     
     //实时时间计算,进行一次循环的时间间隔为worldInfo节点下basicTimeStep的属性值
     t += (double)TIME_STEP / 1000.0;
     
    
  };
  
  //清理
  wb_robot_cleanup();
  
  return 0;
}

      控制器已经生产好了,需要将控制器与机器人进行绑定,确定你的仿真时间是否在0时刻,不是的话点击仿真视图的重置按钮(中间的仿真视图的,不是代码编辑器的.),在机器人的节点下,点击controller节点,选择之前生成的控制器。选择my_controller。(这里我之前命名的是my_robot)

      

 

      到这里还差最后一步, 文件 -> 保存世界 , 如果仿真时间不为0时刻无法保存。保存完成后,再一次点击仿真视图的重置按钮。点击仿真按钮,机器人就动起来了。

    

     今天发现自己的文章被删除原链接转载,好气哦,转载了下面还加这个,好气哦,刚开始写帖子,简直毁灭我的积极性。

 

 

     补充一点,在仿真中,除了位置控制,有时需要用到电机的其他控制方法。其他的控制函数如下:

     速度控制:wb_servo_set_velocity()

     加速度控制:wb_servo_set_acceleration()

     详细了解可以查看reference 3.42-Servo