标题: 四自由度并联四足机器狗 [打印本页]
作者: wurenakihi    时间: 2018-6-25 00:38
标题: 四自由度并联四足机器狗
（分享资料）内含机械自搭的机器小狗的3D模型样本，还附带了arduino的驱动程。这个机器小狗只含四个舵机，通过修改舵机的参数来实现机器狗的前进和后退。

  1. #include <Servo.h>

  3. #define servo_speed 20   //servo_speed
  4. #define servo_speed2 45
  5. #define action_delay 350 //action_delay
  6. Servo myServo[4];
  7. int num = 10;
  8. int servo_port[4]={4,11,3,12};   //servo_pin
  9. int servo_num = sizeof(servo_port)/sizeof(servo_port[0]);  //servo_pin length
  10. float value_init[4]={90, 90, 90, 90}; //各个舵机的初始位置  



  14. void setup() {
  15.   // put your setup code here, to run once:
  16.   Serial.begin(9600);
  17.   for(int i=0; i<servo_num; i++)
  18.   {
  19.     ServoGo(i, value_init[i]);
  20.   }
  21. }

  23. void loop() {
  24.   // put your main code here, to run repeatedly:
  25. for(int i=0;i<1;i++)
  26. {
  27.   left_right_bias();
  28.   delay(100);
  29.   forward_back_lie_down();
  30.   delay(100);
  31. }
  32. //while(1)
  33. //{
  34. // walk();
  35. //}
  36. while(1)
  37. {
  38. test();
  39. }
  40. }


  43. void test()
  44. {
  45.   servo_move(90,60,90,90);
  46.   servo_move(120,60,120,90);
  47.   servo_move(120,60,120,120);
  48.   servo_move(90,90,90,120);
  49. }


  52. void walk()
  53. {
  54.   servo_move(90,60,90,90);
  55.   servo_move(110,60,114,90);
  56.   servo_move(110,60,114,130);
  57.   servo_move(60,90,90,130);   
  58. }

  60. void forward_back_lie_down()
  61. {
  62.   servo_move2(90, 90, 90, 90);
  63.   servo_move2(103,29,75,165);
  64.   servo_move2(90, 90, 90, 90);
  65.   servo_move2(90,156,90,20);
  66.   servo_move2(90, 90, 90, 90);  
  67. }

  69. void left_right_bias()
  70. {
  71.   servo_move2(90, 90, 90, 90);
  72.   servo_move2(47,136,57,119);
  73.   servo_move2(90, 90, 90, 90);
  74.   servo_move2(116,54,147,74);
  75.   servo_move2(90, 90, 90, 90);
  76. }

  78. void ServoStart(int which)
  79. {
  80.   if(!myServo[which].attached())myServo[which].attach(servo_port[which]);
  81.   pinMode(servo_port[which], OUTPUT);
  82. }


  85. void ServoStop(int which)
  86. {
  87.   myServo[which].detach();
  88.   digitalWrite(servo_port[which],LOW);
  89. }

  91. void ServoGo(int which , int where)
  92. {
  93.   if(where!=200)
  94.   {
  95.     if(where==201) ServoStop(which);
  96.     else
  97.     {
  98.       ServoStart(which);
  99.       myServo[which].write(where);
  100.     }
  101.   }
  102. }

  104. void servo_move(float value0, float value1, float value2, float value3)
  105. {
  106.   
  107.   float value_arguments[] = {value0, value1, value2, value3};
  108.   float value_delta[servo_num];
  109.   
  110.   for(int i=0;i<servo_num;i++)
  111.   {
  112.     value_delta[i] = (value_arguments[i] - value_init[i]) / num;
  113.   }
  114.   
  115.   for(int i=0;i<num;i++)
  116.   {
  117.     for(int k=0;k<servo_num;k++)
  118.     {
  119.       value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];
  120.     }
  121.    
  122.     for(int j=0;j<servo_num;j++)
  123.     {
  124.       ServoGo(j,value_init[j]);
  125.     }
  126.     delay(servo_speed);
  127.   }
  128. }

  130. void servo_move2(float value0, float value1, float value2, float value3)
  131. {
  132.   
  133.   float value_arguments[] = {value0, value1, value2, value3};
  134.   float value_delta[servo_num];
  135.   
  136.   for(int i=0;i<servo_num;i++)
  137.   {
  138.     value_delta[i] = (value_arguments[i] - value_init[i]) / num;
  139.   }
  140.   
  141.   for(int i=0;i<num;i++)
  142.   {
  143.     for(int k=0;k<servo_num;k++)
  144.     {
  145.       value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];
  146.     }
  147.    
  148.     for(int j=0;j<servo_num;j++)
  149.     {
  150.       ServoGo(j,value_init[j]);
  151.     }
  152.     delay(servo_speed2);
  153.   }
  154. }
复制代码


四自由度并联四足.rar

581.56 KB, 下载次数: 18, 下载积分: 黑币 -5

作者: akkamk07    时间: 2019-5-7 09:37
收藏了好东西



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1