TFmini与舵机结合的机器人小车避障应用方案

[benewake]

1.试验设备及接线
1.1实验设备
• MiniQ 桌面机器人底盘

• 底盘直径：122mm
• 轮子直径：42mm
• 底盘高度：15mm
• 兼容 Arduino 标准板及 Romeo 控制器固定孔
• 电机参数：
  • N20 电机电压：3-9V
  • 无负载转速：13000rpm
  • 50：1 减速箱
  • 260rpm@6V
  • 40mA@6V
  • 360mA 堵转@6V
  • 10 盎司英寸扭矩@6V

• Romeo 三合一 Arduino 兼容控制器

• 采用 Atmel Atmega328 单片机
• Arduino UNO bootloader
• 完全兼容 Aruduino UNO 的端口布局
• 集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
• 支持 5 组 I2C 总线接口
• 支持两路电机驱动，峰值电流 2A，4 个控制口使用跳线切换
• 外部输入电压范围：6V~20V
• 更详细的参数介绍详见附录的网页地址。

• MiniQ 小车上层安装板

• Benewake TFmini 标版

TFmini 详细参数见 TFmini 使用说明。

• 9g 舵机

1.2接线

2.小车避障原理
小车启动后，小车开始向前运动。当雷达探测到前方阈值内有障碍物时，小车停止运动，开始左右扫描寻路。舵机搭载 TFmini 从 90°开始向 180°扫描，然后从 180°向 0°扫描。

当扫描方向无障碍物时，小车向此方向转向，舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线，则小车后退，舵机回正。
逻辑流程图如下所示：

3.注意事项
• 当前避障原理模型只用来抛砖引玉，探索用 TFmini 避障的可行性，并不能大范围的适用于大规模的商业场景，如有需要，应以专业软件开发人员的代码为准。
• 搭载的外部电源过重时，会影响小车车轮的摩擦力，可能两个车轮的转速不一致，导致小车并不能按照轨迹行驶。
• 小车车轮在光滑地面有可能造成空转的现象，导致小车不能走直线。
• 如果单独对 TFmini 外部供电，则需将外部电源和控制板共地处理。
• 如果搭载更高复杂度的程序，要考虑芯片的能力，当前开发板在跑程序时已经发现会有卡顿的现象。
4.附录
4.1代码
#include <Servo.h>
Servo myservo;
int pos=90; //定义舵机角度
bool flag=true;//定义舵机转向
float dist_f;//定义 foward 方向距离
float dist_s;//定义 sideway 方向距离
int E1=5; //定义 M1 使能
int E2=6; //定义 M2 使能
int M1=4; //定义 M1 控制
int M2=7; //定义 M2 控制
int temp_distance =0;
/**

• 双轮停止
  /
  void brake(void){
  digitalWrite(E1,LOW); //给 E1 低电平
  digitalWrite(E2,LOW); //给 E2 低电平
  }
  /*
• 双轮前进
  /
  void advance(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,LOW);
  analogWrite(E2,b);
  digitalWrite(M2,LOW);
  }
  /*
• 双轮后退
  /
  void back(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,HIGH);
  analogWrite(E2,b);
  digitalWrite(M2,HIGH);
  }
  /*
• 左转
  /
  void turn_L(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,LOW);
  analogWrite(E2,b);
  digitalWrite(M2,HIGH);
  }
  /*
• 右转
  /
  void turn_R(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,HIGH);
  analogWrite(E2,b);
  digitalWrite(M2,LOW);
  }
  /*
• 读取 TFmini 测量结果
  /
  void getTFminiData(int distance, int* strength) {
  static char i = 0;
  char j = 0;
  int checksum = 0;
  static int rx[9];
  if(Serial.available()) {
  rx = Serial.read();
  if(rx[0] != 0x59) {
  i = 0;
  } else if(i == 1 && rx[1] != 0x59) {
  i = 0;
  } else if(i == {
  for(j = 0; j < 8; j++) {
  checksum += rx[j];
  }
  /*
  if(rx[8] == (checksum % 256)) {
  *distance = rx[2] + rx[3] * 256;
  strength = rx[4] + rx[5] * 256;
  }/
  *distance = rx[2] + rx[3] * 256;
  strength = rx[4] + rx[5] * 256;
  i = 0;
  } else {
  i++;
  }
  }
  }
  void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  //舵机的插口在 4
  myservo.attach(4);
  brake();
  /
• 将雷达指向前方
  /
  myservo.write(pos);
  /
• 设置轮胎电机输出口
  /
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  delay(10);
  }
  void loop() {
  /
• 读数一次
  /
  int distance = 0;
  int strength = 0;
  getTFminiData(&distance, &strength);
  while(!distance) {
  getTFminiData(&distance, &strength);
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print("cm ");
  Serial.print("strength: ");
  Serial.println(strength);
  }
  /
• 设置 30CM 阈值
  /
  if(distance <= 30 && distance > 0){
  temp_distance = distance;
  }
  delay(10);
  /
• 判断读数距离
• 如果度数距离小于阈值，则停车，开始向左向右扫描，直到扫描出有空隙可以走，然后车轮转弯，然后扫描器回正
• 如果读数距离大于阈值，则开车
  /
  if(temp_distance <= 30 && temp_distance >= 0){
  brake();
  /
• 判断当前舵机应该向左还是向右转
  /
  if(flag){
  if(pos<170){
  pos=pos+45;
  }else{
  flag = false;
  }
  /
• 如果探测距离大于阈值，则舵机回正，小车转向
  /
  if(distance > 32){
  pos = 90;
  myservo.write(pos);
  delay(1200);
  //判断小车回正方向
  if(pos >= 90){
  turn_L(35,35);
  }else{
  turn_R(35,35);
  }
  delay(250);
  temp_distance = distance;
  }
  /
• 如果探测距离小于阈值，则继续扫描
  /
  else{
  myservo.write(pos);
  delay(1200);
  }
  }else{
  if(pos>10){
  pos=pos-45;
  }else{
  flag=true;
  }
  /
• 如果探测距离大于阈值，则舵机回正，小车转向
  /
  if(distance > 32){
  pos = 90;
  myservo.write(pos);
  delay(1200);
  //判断小车回正方向
  if(pos >= 90){
  turn_L(35,35);
  }else{
  turn_R(35,35);
  }
  delay(250);
  temp_distance = distance;
  }
  /
• 如果探测距离小于阈值，则继续扫描
  /
  else{
  myservo.write(pos);
  delay(1200);
  }
  }
  }
  /
• 如果前方没有障碍物，直行
  /
  else if(distance > 32 && distance < 1200){
  advance(35,35);
  }
  /
• 如果雷达挂了，小车停止
  */
  else if(distance == -3){
  brake();
  }
  }