人车混合机器人实现身体平衡功能

机器谱

1. 功能说明
       在R035c样机上安装一个 六轴陀螺仪传感器【https://www.robotway.com/h-col-137.html】 ，本文示例将实现机器人身体平衡功能。当用手把机器人人形部分摆动到左侧时，人形部分会自动恢复到中间；当用手把机器人人形部分摆动到右侧时，人形部分也会自动恢复到中间。

2. 结构说明
       R035c样机主要是由 【R022】四轮四驱底盘【https://www.robotway.com/h-col-127.html】 上搭载一个人形部分机构组成的。

3. 电子硬件
       在这个示例中，我们采用了以下硬件，请大家参考：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
传感器	六轴陀螺仪
电池	7.4V锂电池

电路连接：
       ① 六轴陀螺仪传感器（GND、VCC、RX、TX）连接在Bigfish扩展板的Gnd、Vcc（3.3v）、RX、TX；
       ② 机器人身体中间部分的舵机连接在Bigfish扩展板的（Gnd、Vcc、D3）端口。

4. 功能实现
       编程环境：Arduino 1.8.19
       实现思路：实现机器人身体平衡功能。当用手把机器人人形部分摆动到左侧时，人形部分会自动恢复到中间；当用手把机器人人形部分摆动到右侧时，人形部分也会自动恢复到中间。
将参考例程（Robot_body_balance.ino）下载到主控板：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-04-25 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. /*   
    
14. 
    
15.     功能：机器人身体平衡
    
16. 
    
17.     接线：陀螺仪传感器(GND、VCC、RX、TX)接在扩展板的( Gnd、Vcc（3.3v）、RX、TX)；
    
18. 
    
19.               机器人身体中间部分的舵机接在扩展板的（Gnd、Vcc、D3）；
    
20. 
    
21.     操作：当用手把机器人人形部分摆动到左侧，人形部分会自动恢复到中间；
    
22. 
    
23.               当用手把机器人人形部分摆动到右侧，人形部分会自动恢复到中间；
    
24. 
    
25.     注意：一定要等人形部分的舵机不转动了，再手动摆动机器人；
    
26. 
    
27. */
    
28. 
    
29. #include<Servo.h>   //调用舵机库函数
    
30. 
    
31. #include<Math.h>
    
32. 
    
33. Servo myservo;
    
34. 
    
35. #define myservopin 3                  //机器人身体中间部分的舵机引脚号
    
36. 
    
37. //陀螺仪的相关参数
    
38. 
    
39. #define Gyroscope_left_LimitAngle_X    0.50   //读取到陀螺仪 X 轴向左偏的极限数值
    
40. 
    
41. #define Gyroscope_Right_LimitAngle_X   -0.65   //读取到陀螺仪 X 轴向右偏的极限数值
    
42. 
    
43. #define Gyroscope_Middle_LimitAngle_X   -0.01    //读取到陀螺仪 X 轴平放时的数值
    
44. 
    
45. //机器人人形部分姿态参数
    
46. 
    
47. #define Servo_One_Min_Angle 66      //机器人身体中间部分的舵机左偏极限角度
    
48. 
    
49. #define Servo_One_Max_Angle 118     //机器人身体中间部分的舵机右偏极限角度
    
50. 
    
51. #define Servo_One_Middle_Angle 90   //机器人身体中间部分的舵机处于中间位置角度
    
52. 
    
53. #define Servo_Speed 10              //舵机速度
    
54. 
    
55. 
    
56. void setup()
    
57. 
    
58. {
    
59. 
    
60.    Serial.begin(115200);           //打开串口，并设置波特率为115200
    
61. 
    
62.    myservo.attach(myservopin);
    
63. 
    
64. }
    
65. 
    
66. 
    
67. void loop()
    
68. 
    
69. {
    
70. 
    
71.    Get_gyroscope_And_Control();   //根据陀螺仪传感器的数据实现姿态跟随
    
72. 
    
73. }

复制代码

判断机器人人形部分姿态的参考程序（Gyroscope_Device.ino）如下：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-04-25 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. unsigned char Re_buf[11],counter=0;
    
14. 
    
15. unsigned char sign=0;
    
16. 
    
17. float a[3],w[3],angle[3],T;
    
18. 
    
19. int gyroscope_middle_position_min = (Gyroscope_Middle_LimitAngle_X-0.2) * 100;
    
20. 
    
21. int gyroscope_middle_position_max = (Gyroscope_Middle_LimitAngle_X+0.2) * 100;
    
22. 
    
23. int gyroscope_left_position_min   =   Gyroscope_left_LimitAngle_X * 100;
    
24. 
    
25. int gyroscope_left_position_max   =   gyroscope_middle_position_max;
    
26. 
    
27. int gyroscope_right_position_min   =   gyroscope_middle_position_min;
    
28. 
    
29. int gyroscope_right_position_max   =   Gyroscope_Right_LimitAngle_X * 100;
    
30. 
    
31. unsigned long record_time = 0;
    
32. 
    
33. int count = 0;
    
34. 
    
35. 
    
36. void Get_gyroscope_And_Control()
    
37. 
    
38. {
    
39. 
    
40.    int gyroscope_acc_data[2]={0,0};
    
41. 
    
42.    int map_data= 0;
    
43. 
    
44.    int servo_angle = 0;
    
45. 
    
46.    if(sign)
    
47. 
    
48.    {
    
49. 
    
50.      sign=0;
    
51. 
    
52.      if(Re_buf[0]==0x55)      //检查帧头
    
53. 
    
54.      {  
    
55. 
    
56. switch(Re_buf [1])
    
57. 
    
58. {
    
59. 
    
60. case 0x51:
    
61. 
    
62.                {
    
63. 
    
64.                 record_time = millis();
    
65. 
    
66. a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
    
67. 
    
68. a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
    
69. 
    
70. a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
    
71. 
    
72. T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
    
73. 
    
74.                 map_data = a[0] * 100;
    
75. 
    
76.                 //把陀螺仪的沿X轴的加速度值转为舵机的角度
    
77. 
    
78.                 if(   ( millis() - record_time ) < 3000 )
    
79. 
    
80.                 {
    
81. 
    
82.                   count ++;
    
83. 
    
84.                   //每隔1s，判断机器人人形部分的姿态；
    
85. 
    
86.                   if(count >= 100)   
    
87. 
    
88.                   {
    
89. 
    
90.                     count = 0;
    
91. 
    
92.                     //默认机器人处于平衡态（如机器人处于直立状态）
    
93. 
    
94.                     if(map_data>=gyroscope_middle_position_min && map_data<=gyroscope_middle_position_max)   //i am state
    
95. 
    
96.                     {
    
97. 
    
98.                       ServoStop();   
    
99. 
    
100.                     }
     
101. 
     
102.                     //检测到机器人人形部分摆动到左侧，人形部分会自动恢复到平衡态(中间)；
     
103. 
     
104.                     if(map_data>gyroscope_left_position_max && map_data< gyroscope_left_position_min)   //i am left
     
105. 
     
106.                     {  
     
107. 
     
108.                       myservo.attach(myservopin);
     
109. 
     
110.                       servo_angle = map( abs(map_data), abs(gyroscope_left_position_min), abs(gyroscope_left_position_max), Servo_One_Min_Angle, Servo_One_Middle_Angle);
     
111. 
     
112.                       Servo_move(servo_angle, Servo_One_Middle_Angle);
     
113. 
     
114.                       delay(300);ServoStop();
     
115. 
     
116.                     }
     
117. 
     
118.                     //检测到机器人人形部分摆动到右侧，人形部分会自动恢复到平衡态(中间)；
     
119. 
     
120.                     if(map_data>gyroscope_right_position_max && map_data< gyroscope_right_position_min)   //i am right
     
121. 
     
122.                     {
     
123. 
     
124.                       myservo.attach(myservopin);
     
125. 
     
126.                       servo_angle = map( abs(map_data), abs(gyroscope_right_position_min), abs(gyroscope_right_position_max), Servo_One_Middle_Angle, Servo_One_Max_Angle);
     
127. 
     
128.                       Servo_move(servo_angle, Servo_One_Middle_Angle);delay(300);
     
129. 
     
130.                       ServoStop();
     
131. 
     
132.                     }         
     
133. 
     
134.                   }      
     
135. 
     
136.                 }         
     
137. 
     
138.                }break;
     
139. 
     
140. }
     
141. 
     
142.     }
     
143. 
     
144.   }  
     
145. 
     
146. }
     
147. 
     
148. 
     
149. void serialEvent() {
     
150. 
     
151.   while (Serial.available()) {   
     
152. 
     
153.     Re_buf[counter]=(unsigned char)Serial.read();
     
154. 
     
155.     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头            
     
156. 
     
157.     counter++;      
     
158. 
     
159.     if(counter==11)             //接收到11个数据
     
160. 
     
161.     {   
     
162. 
     
163.        counter=0;               //重新赋值，准备下一帧数据的接收
     
164. 
     
165.        sign=1;
     
166. 
     
167.     }     
     
168. 
     
169.   }
     
170. 
     
171. }
     
172. 
     
173. 
     
174. void ServoStop()
     
175. 
     
176. {
     
177. 
     
178.   myservo.detach();
     
179. 
     
180.   digitalWrite(myservopin,LOW);
     
181. 
     
182. }
     
183. 
     
184. 
     
185. void Servo_move(int start_angle, int finish_angle)
     
186. 
     
187. {
     
188. 
     
189.   int flag = 0;
     
190. 
     
191.   if( (start_angle - finish_angle) >0 ){ flag = -1;}
     
192. 
     
193.   else { flag = 1; }
     
194. 
     
195.   for( int i=0; i< abs(start_angle - finish_angle);i++){
     
196. 
     
197.     myservo.write( start_angle + flag*i );
     
198. 
     
199.     delay(Servo_Speed);
     
200. 
     
201.   }
     
202. 
     
203. }

复制代码

5. 扩展样机
       本样机可以有一些扩展，如减少样机底盘上的2个直流驱动轮，或者将样机底盘上的直流驱动轮替换成舵机，如下图所示：

6. 资料下载
资料内容：
①身体平衡-程序源代码
②身体平衡-样机3D文件
资料下载地址：https://www.robotway.com/h-col-213.html

想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com