本帖最后由 adream 于 2016-4-20 16:10 编辑

其实,超声波模块和蓝牙模块可以说是遥控平衡车的一种方式,即模块作为外部检测器件(输入)和平衡车运动控制(输出),其中蓝牙模块是通过APP上采集摇杆的值,从而mbot ranger做出相应的运动。而超声波模块是作为检测外部障碍物距离,从而改变当前的运动状态。

首先开了宏定义可以实现平衡车壁障功能,注释则关闭功能,其中壁障有两个开关宏(FOLLOW)跟随 和(FORWARD)自动前进。
#define BARRIER  //壁障
跟随功能---当超声波检测到障碍物距离为10cm--50cm时前进行驶,就类似于遛狗一样的跟随了。
#define FOLLOW //跟随状态
自动前进功能---当超声波检测到障碍物距离大于10cm时都是前进行驶,满足的概率很大,所以几乎都是往前行驶的。
#define FORWARD//自动前进状态

定义陀螺仪对象
MeGyro gyro(1,0x69);      //板载陀螺仪

定义超声波模块,并接port9
MeUltrasonicSensor us(PORT_9);

定义左右编码电机测速对象
MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);

以下说明一下mbot ranger在平衡车模式下使用超声波实现壁障的原理和代码解释。
balanced_model();//平衡车控制函数

void balanced_model(void)
{
    //检测当前角度小于正负30度时执行平衡控制,否则关闭电机。
    if(abs(gyro.getAngleX()) < 30) {
      //angle control
      if((millis() - lasttime_angle) > 10) {
        last_speed_setpoint_filter  = last_speed_setpoint_filter  * 0.8;
              l ast_speed_setpoint_filter  += PID_speed.Setpoint * 0.2;
              PID_angle.Setpoint = RELAX_ANGLE - PID_speed.Output;

              last_turn_setpoint_filter  =   last_turn_setpoint_filter * 0.9;
              last_turn_setpoint_filter  +=  PID_turn.Setpoint * 0.1;
              PID_turn.Output        =   last_turn_setpoint_filter;

        PID_angle_compute();
        lasttime_angle = millis();
      }   

      //speed control
      if((millis() - lasttime_speed) > 100) {
        PID_speed_compute();
        lasttime_speed = millis();
      }

#ifdef BARRIER      
      //distance control
    //在循环周期内,每150ms调用一次,以防止超声波模块控制对直立控制的干扰,也就是说让壁障的运动更加顺滑一点。  
if((millis() - lasttime_distance) > 150) {
        ultrCarProcess();
        lasttime_distance = millis();
      }
#endif      
  }
  else {
    Encoder_1.setMotorPwm(0);
    Encoder_2.setMotorPwm(0);
  }
}
//超声波模块测距函数和控制模块
void ultrCarProcess(void)
{
  static char turn = 1;//左右转标志,这样设置的目的是壁障左右转轮流切换
  int16_t distance=0;//存放测距距离,距离单位为cm   
  distance = us.distanceCm();

#ifdef FORWARD  
  //当距离大于10cm时,前进速度设为40(可以根据个人喜好进行调整,速度为60应该已经极限了)
  if(distance > 10) {                    //前进
    PID_speed.Setpoint = 40;
  }
#endif
#ifdef FOLLOW
//当距离大于等于10cm且小于等于50cm时,前进速度设为40
  if((distance > 10) && (distance <= 50)) {  //前进
    PID_speed.Setpoint = 35;
  }
#endif
  //当距离小于等于10cm且大于等于5cm
  else if(distance <= 10 && distance >= 5) { //后退并转向
    PID_turn.Setpoint = 30;
    PID_speed.Setpoint = -20;
  }
  //当距离小于5cm且大于等于1cm,电机转向速度为40,转向值应该可以去到60,还可以再大点,反正你会眼花缭乱,但是转向太激烈就会失去其他运动功能了,请慎重改变!
  else if(distance < 5 && distance >= 1) {   //转向并退后
    if(turn == 1) {
      PID_turn.Setpoint = 40;
      PID_speed.Setpoint = -10;
      turn = 2;
    }
    else if(turn == 2) {
      turn = 1;
      PID_turn.Setpoint = -40;
      PID_speed.Setpoint = -10;
    }
  }
}

//角度环控制函数,主要是控制平衡车的直立状态
void PID_angle_compute(void)   //PID
{
  CompAngleX = -gyro.getAngleX();
  double error = CompAngleX - PID_angle.Setpoint;
  PID_angle.Integral += error;                                                                                                         
  PID_angle.differential = angle_speed;
  PID_angle.Output = PID_angle.P * error + PID_angle.I * PID_angle.Integral + PID_angle.D * PID_angle.differential;
  if(PID_angle.Output > 0)
  {
    PID_angle.Output = PID_angle.Output + PWM_MIN_OFFSET;
  }
  else
  {
    PID_angle.Output = PID_angle.Output - PWM_MIN_OFFSET;
  }

  double pwm_left = PID_angle.Output - PID_turn.Output ;
  double pwm_right = -PID_angle.Output - PID_turn.Output;

  pwm_left = constrain(pwm_left, -255, 255);
  pwm_right = constrain(pwm_right, -255, 255);

  Encoder_1.setMotorPwm(pwm_left);
  Encoder_2.setMotorPwm(pwm_right);
}
//速度环控制函数,主要是通过调整角度平衡点来运动控制或者稳定直立控制
void PID_speed_compute(void)
{
  speed_now = (Encoder_2.GetCurrentSpeed() - Encoder_1.GetCurrentSpeed())/2;

  last_speed_setpoint_filter  = last_speed_setpoint_filter  * 0.8;
  last_speed_setpoint_filter  += PID_speed.Setpoint * 0.2;

  double error = speed_now - last_speed_setpoint_filter;
  PID_speed.Integral += error;

  PID_speed.Integral = constrain(PID_speed.Integral , -1500, 1500);
  PID_speed.Output = PID_speed.P * error + PID_speed.I * PID_speed.Integral;
  PID_speed.Output = constrain(PID_speed.Output , -15.0, 15.0);
  speed_Integral_average = 0.8 * speed_Integral_average + 0.2 * PID_speed.Integral;

  PID_angle.Setpoint =  RELAX_ANGLE - PID_speed.Output;
}

1.源码分析附件:

mot_ranger代码说明.pdf

89.01 KB, 下载次数: 870

2人收藏
5 条回帖
面包板新手会员2016-6-26 22:23:29
顶啊,终于解决我用arduino ide编程的问题,原来要先这样定义。
adream新手会员2016-6-27 17:58:23
面包板 发表于 2016-6-26 22:23
顶啊,终于解决我用arduino ide编程的问题,原来要先这样定义。

什么东西,不太理解你的意思,要用官方库先必须包含头文件和库
面包板新手会员2016-6-27 18:09:01
adream 发表于 2016-6-27 17:58
什么东西,不太理解你的意思,要用官方库先必须包含头文件和库

您好,库文件你已经提供了,请问ranger主控板的头文件是什么?
adream新手会员2016-6-27 18:15:50
面包板 发表于 2016-6-27 18:09
您好,库文件你已经提供了,请问ranger主控板的头文件是什么?

#include <Arduino.h>
#include <avr/wdt.h>
#include <MeAuriga.h>
#include "MeEEPROM.h"
#include <Wire.h>
#include <SoftwareSerial.h>

你有测试过了吗?我现在的库不算是最新的了,但是要想实现自平衡是足够的
面包板新手会员2016-6-30 17:02:12
adream 发表于 2016-6-27 18:15
#include
#include
#include

好的,麻烦了,我们正在填志愿,我还在看各种教程,已经组装好坦克模式。我打算做全向车。
需要登陆后才可进行回复 登录

返回顶部
现在加入我们,注册一个账号 账号登陆 QQ账号登陆 微博账号登陆