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

1. 功能说明

      在R035c样机上安装一个六轴陀螺仪传感器，本文示例将实现机器人身体平衡功能。当用手把机器人人形部分摆动到左侧时，人形部分会自动恢复到中间；当用手把机器人人形部分摆动到右侧时，人形部分也会自动恢复到中间。

​

 2. 结构说明

      R035c样机主要是由四轮四驱底盘上搭载一个人形部分机构组成的。

​

 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）下载到主控板： 

【详细源程序代码内容请见 人车混合机器人-身体平衡】

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-04-25 https://www.robotway.com/

  ------------------------------*/

/*   

    功能：机器人身体平衡

    接线：陀螺仪传感器(GND、VCC、RX、TX)接在扩展板的( Gnd、Vcc（3.3v）、RX、TX)；

              机器人身体中间部分的舵机接在扩展板的（Gnd、Vcc、D3）；

    操作：当用手把机器人人形部分摆动到左侧，人形部分会自动恢复到中间；

              当用手把机器人人形部分摆动到右侧，人形部分会自动恢复到中间；

    注意：一定要等人形部分的舵机不转动了，再手动摆动机器人；

*/

#include<Servo.h>   //调用舵机库函数

#include<Math.h>

Servo myservo;

#define myservopin 3                  //机器人身体中间部分的舵机引脚号

//陀螺仪的相关参数

#define Gyroscope_left_LimitAngle_X    0.50   //读取到陀螺仪 X 轴向左偏的极限数值

#define Gyroscope_Right_LimitAngle_X   -0.65   //读取到陀螺仪 X 轴向右偏的极限数值

#define Gyroscope_Middle_LimitAngle_X   -0.01    //读取到陀螺仪 X 轴平放时的数值

//机器人人形部分姿态参数

#define Servo_One_Min_Angle 66      //机器人身体中间部分的舵机左偏极限角度

#define Servo_One_Max_Angle 118     //机器人身体中间部分的舵机右偏极限角度

#define Servo_One_Middle_Angle 90   //机器人身体中间部分的舵机处于中间位置角度

#define Servo_Speed 10              //舵机速度


void setup()

{

   Serial.begin(115200);           //打开串口，并设置波特率为115200

   myservo.attach(myservopin);

}


void loop()

{

   Get_gyroscope_And_Control();   //根据陀螺仪传感器的数据实现姿态跟随

}

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

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-04-25 https://www.robotway.com/

  ------------------------------*/

unsigned char Re_buf[11],counter=0;

unsigned char sign=0;

float a[3],w[3],angle[3],T;

int gyroscope_middle_position_min = (Gyroscope_Middle_LimitAngle_X-0.2) * 100;

int gyroscope_middle_position_max = (Gyroscope_Middle_LimitAngle_X+0.2) * 100;

int gyroscope_left_position_min   =   Gyroscope_left_LimitAngle_X * 100;

int gyroscope_left_position_max   =   gyroscope_middle_position_max;

int gyroscope_right_position_min   =   gyroscope_middle_position_min;

int gyroscope_right_position_max   =   Gyroscope_Right_LimitAngle_X * 100;

unsigned long record_time = 0;

int count = 0;


void Get_gyroscope_And_Control()

{

   int gyroscope_acc_data[2]={0,0};

   int map_data= 0;

   int servo_angle = 0;

   if(sign)

   {

     sign=0;

     if(Re_buf[0]==0x55)      //检查帧头

     {  

switch(Re_buf [1])

{

case 0x51:

               {

                record_time = millis();

a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;

a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;

a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;

T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;

                map_data = a[0] * 100;

                //把陀螺仪的沿X轴的加速度值转为舵机的角度

                if(   ( millis() - record_time ) < 3000 )

                {

                  count ++;

                  //每隔1s，判断机器人人形部分的姿态；

                  if(count >= 100)   

                  {

                    count = 0;

                    //默认机器人处于平衡态（如机器人处于直立状态）

                    if(map_data>=gyroscope_middle_position_min && map_data<=gyroscope_middle_position_max)   //i am state

                    {

                      ServoStop();   

                    }

                    //检测到机器人人形部分摆动到左侧，人形部分会自动恢复到平衡态(中间)；

                    if(map_data>gyroscope_left_position_max && map_data< gyroscope_left_position_min)   //i am left

                    {  

                      myservo.attach(myservopin);

                      servo_angle = map( abs(map_data), abs(gyroscope_left_position_min), abs(gyroscope_left_position_max), Servo_One_Min_Angle, Servo_One_Middle_Angle);

                      Servo_move(servo_angle, Servo_One_Middle_Angle);

                      delay(300);ServoStop();

                    }

                    //检测到机器人人形部分摆动到右侧，人形部分会自动恢复到平衡态(中间)；

                    if(map_data>gyroscope_right_position_max && map_data< gyroscope_right_position_min)   //i am right

                    {

                      myservo.attach(myservopin);

                      servo_angle = map( abs(map_data), abs(gyroscope_right_position_min), abs(gyroscope_right_position_max), Servo_One_Middle_Angle, Servo_One_Max_Angle);

                      Servo_move(servo_angle, Servo_One_Middle_Angle);delay(300);

                      ServoStop();

                    }         

                  }       

                }         

               }break;

}

    }

  }  

}


void serialEvent() {

  while (Serial.available()) {   

    Re_buf[counter]=(unsigned char)Serial.read();

    if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头             

    counter++;       

    if(counter==11)             //接收到11个数据

    {   

       counter=0;               //重新赋值，准备下一帧数据的接收

       sign=1;

    }     

  }

}


void ServoStop()

{

  myservo.detach();

  digitalWrite(myservopin,LOW);

}


void Servo_move(int start_angle, int finish_angle)

{

  int flag = 0;

  if( (start_angle - finish_angle) >0 ){ flag = -1;}

  else { flag = 1; }

  for( int i=0; i< abs(start_angle - finish_angle);i++){

    myservo.write( start_angle + flag*i );

    delay(Servo_Speed);

  }

}

 5. 扩展样机

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

​