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

北醒

, ^$ e+ [. Z( v; L% _. r; T3 r% G; v

1.试验设备及接线
1 G6 B; `0 K& L# _& f) u1.1实验设备  

·        MiniQ 桌面机器人底盘
6 d7 u# e5 g: n, ?3 p7 @; {8 m0 B   
- 底盘直径：122mm
- 轮子直径：42mm
6 E: n' _; _. V3 F3 Z- 底盘高度：15mm
- 兼容 Arduino 标准板及 Romeo 控制器固定孔
4 F9 B9 P( l. b( O2 z- 电机参数：

·           N20 电机电压：3-9V

·          无负载转速：13000rpm

·          50：1 减速箱

·          260rpm@6V

·          40mA@6V

·          360mA 堵转@6V

·          10 盎司英寸扭矩@6V

                                                      

·        Romeo 三合一 Arduino 兼容控制器

: P" z  D, f: j% T- 采用 Atmel Atmega328 单片机
- Arduino UNO bootloader
- 完全兼容 Aruduino UNO 的端口布局
$ ^% a5 \- s6 {5 y- 集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
- 支持 5 组 I2C 总线接口
. G* ]+ r+ a, \* E! u; D9 F3 {- 支持两路电机驱动，峰值电流 2A，4 个控制口使用跳线切换
- 外部输入电压范围：6V~20V
9 d; Q  [/ N2 d' {- 更详细的参数介绍详见附录的网页地址。
9 m3 A0 ~% v* `6 H+ I" T
+ y+ W* E/ V  z: C* ~1 S

·        MiniQ 小车上层安装板

. Y7 A3 @+ ]3 @2 G5 k

·        Benewake TFmini 标版

TFmini 详细参数见 TFmini 使用说明。
7 A4 i0 m1 b) s1 n3 s4 m# S

·        9g 舵机

0 B' t) z+ o' v% d( l' \1 V1 a* I( Q1.2接线

2.小车避障原理
小车启动后，小车开始向前运动。当雷达探测到前方阈值内有障碍物时，小车停止运动，开始左右扫描寻路。舵机搭载 TFmini 从 90°开始向 180°扫描，然后从 180°向 0°扫描。
: k" F* i9 m% T! ], ~! y/ y  l
: y0 I# w1 _% ~当扫描方向无障碍物时，小车向此方向转向，舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线，则小车后退，舵机回正。
逻辑流程图如下所示：

3.注意事项

·        当前避障原理模型只用来抛砖引玉，探索用 TFmini 避障的可行性，并不能大范围的适用于大规模的商业场景，如有需要，应以专业软件开发人员的代码为准。

·        搭载的外部电源过重时，会影响小车车轮的摩擦力，可能两个车轮的转速不一致，导致小车并不能按照轨迹行驶。

·        小车车轮在光滑地面有可能造成空转的现象，导致小车不能走直线。

·        如果单独对 TFmini 外部供电，则需将外部电源和控制板共地处理。

·        如果搭载更高复杂度的程序，要考虑芯片的能力，当前开发板在跑程序时已经发现会有卡顿的现象。

4.附录
& m; x0 R2 o8 G& H- b4.1代码
#include <Servo.h>
2 v$ w: n  n5 RServo myservo;
int pos=90; //定义舵机角度
bool flag=true;//定义舵机转向
float dist_f;//定义foward 方向距离
float dist_s;//定义sideway 方向距离
9 j# Y( m6 X5 S4 Jint E1=5; //定义 M1 使能
  \" |, m  M% Dint E2=6; //定义 M2 使能
int M1=4; //定义 M1 控制
int M2=7; //定义 M2 控制
  u7 _# Q0 [9 K& tint temp_distance =0;
/**
- M* e( t4 K8 C6 `, t* 双轮停止
" O0 \# O/ ?" ]: K*/
void brake(void){
  B5 K4 Y* s3 C0 BdigitalWrite(E1,LOW); //给 E1 低电平
: o) Q8 q6 D9 @0 q. `digitalWrite(E2,LOW); //给 E2 低电平
1 Q/ v& u* U0 `, E, \. j}
9 ~. P& l# e6 R! E+ N* G/**
* 双轮前进
*/
void advance(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
* 双轮后退
*/
! w: X' C; R8 i% V0 z  e. pvoid back(char a, char b){
4 O. m# Y  o' d" i4 EanalogWrite(E1,a);
& T" t0 }# }! J% A$ \0 q  EdigitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
9 Z7 s6 f' Y* c4 X& Y}
/**
* 左转
1 h# ]$ L. L( J% N*/
void turn_L(char a, char b){
analogWrite(E1,a);
$ L' f- [* @7 R6 A) b  D/ f3 hdigitalWrite(M1,LOW);
( Y' l; E1 N, d! A3 yanalogWrite(E2,b);
6 h, V3 s4 V% `" ]digitalWrite(M2,HIGH);
' R! u3 A) M$ L, W: x/ t}
2 K" w: f3 H( M& G0 ^/**
* 右转
*/
1 e: j8 G0 c" W+ e; Xvoid turn_R(char a, char b){
; _! o. U- c# e7 }" b% vanalogWrite(E1,a);
, N' q; B  P0 rdigitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
! ?/ s3 x- k# C! F4 \' F* 读取 TFmini 测量结果
% R; @- f% {7 z9 d; M*/
void getTFminiData(int* distance, int* strength){
static char i = 0;
* Z/ Y$ m% h; Q- i' ~* y0 wchar j = 0;
8 F, i# Z4 c5 Vint checksum = 0;
static int rx[9];
if(Serial.available()) {
rx = Serial.read();
if(rx[0] != 0x59) {
i = 0;
# v" O& c$ y  y3 m8 ^9 O. k0 U} else if(i == 1 && rx[1] != 0x59) {
i = 0;
; y$ K+ |+ s* B) |6 ?} else if(i == 8) {
for(j = 0; j < 8; j++) {
checksum += rx[j];
}
/*
if(rx[8] == (checksum % 256)) {
*distance = rx[2] + rx[3] * 256;
% P2 W$ y, B0 f4 f' c4 H*strength = rx[4] + rx[5] * 256;
}*/
*distance = rx[2] + rx[3] * 256;
*strength = rx[4] + rx[5] * 256;
i = 0;
* P) \# i* p; M% q" l} else {
i++;
}
3 v: y- x( q8 }}
+ X2 G& R/ L- {6 R- Z8 B}
4 b3 [! [# D5 [* Q. \# K7 t. Vvoid setup() {
* W  B0 e7 C/ d# K' C2 k* ?// put your setup code here, to run once:
% H" f3 a# P, W, C" n3 [Serial.begin(115200);
//舵机的插口在 4
& e0 d- i2 K- M* E7 Imyservo.attach(4);
2 f% F1 f8 [, n3 Obrake();
" g$ ^" N- Q( U& c: N8 A- l/*
* 将雷达指向前方
  a% _: D. P3 S2 X9 J0 m" H: b*/
myservo.write(pos);
$ q( r) }4 B. I9 m, |. o/*
! J! B% `' g: S8 i/ v* 设置轮胎电机输出口
( l( {+ q( l' x( i3 Z. t*/
% d% W2 Z1 V! x6 kpinMode(4,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
. [) J2 n9 v" S) Z+ J3 DpinMode(7,OUTPUT);
0 x, ~- }9 d9 y3 i1 Ydelay(10);
}
void loop() {
7 Q* y: y5 V- l1 u/ a4 A( V5 J) l/*
  J1 ^+ L* x. d4 x; I9 A- W* 读数一次
*/
7 V- j* i! ?/ |& D+ Cint distance = 0;
int strength = 0;
getTFminiData(&distance, &strength);
: r8 [$ O" S) T/ c& nwhile(!distance) {
getTFminiData(&distance, &strength);
2 k: d  [1 P! H  tSerial.print("Distance: ");
/ D5 W2 n$ \1 b- NSerial.print(distance);
Serial.print("cm ");
% g' d8 G# h6 PSerial.print("strength: ");
Serial.println(strength);
}
: a2 G4 D+ I! ?* ]7 u7 M9 ~/*
* 设置 30CM 阈值
*/
if(distance <= 30 && distance > 0){
0 m4 `$ A- P3 w! ztemp_distance = distance;
}
delay(10);
( J/ Q4 T. J. Q- K6 i8 N( ^/*
* 判断读数距离
* 如果度数距离小于阈值，则停车，开始向左向右扫描，直到扫描出有空隙可以走，然后车轮转弯，然后扫描器回正
1 ~& k! x# M0 }  B3 h* 如果读数距离大于阈值，则开车
) o; c# H' v0 O% l! ]*/
if(temp_distance <= 30 && temp_distance >= 0){
brake();
( B" p/ H* B- o. P/*
/ v% F  I! N7 i( Y- S* 判断当前舵机应该向左还是向右转
*/
if(flag){
if(pos<170){
+ C  L2 i0 H4 Q! [pos=pos+45;
}else{
flag = false;
}
' d4 C0 R& X) e$ \/*
* 如果探测距离大于阈值，则舵机回正，小车转向
+ c. j2 z: _! s+ I+ d*/
# q; @0 }. f5 v, ~. Yif(distance > 32){
pos = 90;
myservo.write(pos);
delay(1200);
9 q! A+ F2 `. j' [+ n/ A  }. q2 c//判断小车回正方向
if(pos >= 90){
turn_L(35,35);
/ o: {+ r4 T" O+ R$ P5 `2 _}else{
8 C- M/ G& u& T6 h2 aturn_R(35,35);
+ D3 x* z2 t% w+ W- U( E}
delay(250);
temp_distance = distance;
}
/*
* 如果探测距离小于阈值，则继续扫描
6 _+ K, S; E8 B+ D! D9 C+ T3 k# w*/
else{
myservo.write(pos);
delay(1200);
}
& M3 t. I0 F( O2 O" R. E}else{
if(pos>10){
- Z' q! v8 a: R2 L8 t6 lpos=pos-45;
}else{
flag=true;
$ u* G% B' x0 g}
/*
& Y4 [# A! k; P& g* 如果探测距离大于阈值，则舵机回正，小车转向
*/
if(distance > 32){
pos = 90;
myservo.write(pos);
delay(1200);
//判断小车回正方向
& \& W5 ^/ Q2 wif(pos >= 90){
, C' t3 U' I8 T; i- _turn_L(35,35);
; m/ }1 f- J5 J$ F7 U8 z6 c}else{
9 M5 a' T1 ]. y3 Q( G5 ]turn_R(35,35);
}
  L, ~! F/ D" h# h, F" s0 \delay(250);
6 [- I! y$ E8 i& v! k, qtemp_distance = distance;
}
$ q+ C9 y! Y2 v: V6 ^- F, Z/*
6 w' _- y9 {$ k* 如果探测距离小于阈值，则继续扫描
*/
5 w0 A8 t6 L9 }4 _  ^else{
myservo.write(pos);
delay(1200);
}
}
}
6 J! a4 a! y4 L$ G/*
* 如果前方没有障碍物，直行
. @4 {% F3 S  q*/
$ z0 a) Q+ Z: gelse if(distance > 32 && distance < 1200){
( p. v8 ^" M) o/ h( `. dadvance(35,35);
}
/*
* 如果雷达挂了，小车停止
*/
5 W1 ^! L! H  d( Eelse if(distance == -3){
% I- I6 o/ N- V8 @% o1 Ebrake();
}
}

. K& t: x5 p" D& x

jacky401

欢迎分享技术，拒绝广告内容！