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

北醒

1.试验设备及接线
. G8 @# K1 N3 }- g, j. s; ]1.1实验设备  

·        MiniQ 桌面机器人底盘
% U; Q/ x. U. N# t; t% u% n   
- 底盘直径：122mm
+ L! L+ Y$ b8 g/ ~- 轮子直径：42mm
- 底盘高度：15mm
- 兼容 Arduino 标准板及 Romeo 控制器固定孔
- 电机参数：

·           N20 电机电压：3-9V

·          无负载转速：13000rpm

·          50：1 减速箱

·          260rpm@6V

·          40mA@6V

·          360mA 堵转@6V

·          10 盎司英寸扭矩@6V

                                                      

·        Romeo 三合一 Arduino 兼容控制器

- 采用 Atmel Atmega328 单片机
$ Q! ]7 ~  W4 \* \. W- Arduino UNO bootloader
- 完全兼容 Aruduino UNO 的端口布局
- 集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
- 支持 5 组 I2C 总线接口
- 支持两路电机驱动，峰值电流 2A，4 个控制口使用跳线切换
- 外部输入电压范围：6V~20V
- 更详细的参数介绍详见附录的网页地址。
1 \; S' S, P- v" C0 c
* U* _; l4 f: c( y

·        MiniQ 小车上层安装板

0 |( z" ^( y4 ~8 I+ l& j+ q! e' z

·        Benewake TFmini 标版

TFmini 详细参数见 TFmini 使用说明。
& e; b- Y& {! y% f
1 \1 H8 @6 ]; ~9 a0 O

·        9g 舵机

4 n" N% ~* o* H  [$ a& D1.2接线

. h; C; g# U  r  ~- {+ b2.小车避障原理
) j8 R, x6 L# j. ?小车启动后，小车开始向前运动。当雷达探测到前方阈值内有障碍物时，小车停止运动，开始左右扫描寻路。舵机搭载 TFmini 从 90°开始向 180°扫描，然后从 180°向 0°扫描。

当扫描方向无障碍物时，小车向此方向转向，舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线，则小车后退，舵机回正。
逻辑流程图如下所示：
) e8 {& P( X6 w  ~
3.注意事项

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

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

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

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

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

4.附录
; Z( b! ^- f6 b2 @$ y" b  |( Z" m4.1代码
#include <Servo.h>
9 X: p+ n0 f1 B7 u5 s, |3 T' L2 K# ~, EServo myservo;
int pos=90; //定义舵机角度
bool flag=true;//定义舵机转向
& w8 W) v& r- g  D& qfloat dist_f;//定义foward 方向距离
float dist_s;//定义sideway 方向距离
  l7 J% p* Y- q8 H/ r3 K6 T/ uint E1=5; //定义 M1 使能
0 w& T4 W1 M( V- P4 b" Z, @9 dint E2=6; //定义 M2 使能
" `9 v4 L# C7 g" H, R+ vint M1=4; //定义 M1 控制
int M2=7; //定义 M2 控制
int temp_distance =0;
/ k" R: V% U; i( n- O2 F, }/**
* 双轮停止
2 {, P* o8 g8 G/ h9 _& w" X*/
3 I5 O* Q6 L3 K6 evoid brake(void){
; P3 `% s/ I+ a7 o4 @( m3 ZdigitalWrite(E1,LOW); //给 E1 低电平
# }; |0 W3 }* {$ @digitalWrite(E2,LOW); //给 E2 低电平
}
/**
" x( K' K$ q9 b+ w. z$ h9 W* 双轮前进
*/
void advance(char a, char b){
+ |6 I8 O' M: B3 Y( o! ^; NanalogWrite(E1,a);
. a/ g3 U) _1 t% pdigitalWrite(M1,LOW);
: d7 V/ X8 i5 J/ E( {: panalogWrite(E2,b);
& b( y; ?/ m5 i2 U9 n% G4 Z5 e. VdigitalWrite(M2,LOW);
}
/**
  R7 `6 n0 X( Y8 S/ s+ A* 双轮后退
*/
void back(char a, char b){
analogWrite(E1,a);
) o' ^2 J; w2 R4 T, b7 J2 EdigitalWrite(M1,HIGH);
' R$ s0 w$ {& P; eanalogWrite(E2,b);
. I) j) B) k$ v2 G3 S6 R8 XdigitalWrite(M2,HIGH);
) z3 Z' j2 i4 [6 C1 b% B2 g* }}
/**
* 左转
*/
  V( m$ E; b+ F7 m5 @2 A2 Fvoid turn_L(char a, char b){
% E/ K/ J8 p! V7 U. RanalogWrite(E1,a);
4 I  N1 l+ j5 tdigitalWrite(M1,LOW);
analogWrite(E2,b);
) ]( Z( b  ?# j# L# R* H* N# y- ddigitalWrite(M2,HIGH);
/ z* z0 g0 M7 M9 f! T}
/**
" K* W6 P7 p' Z- v' V$ @% I* 右转
( c1 |7 Y$ K/ V3 M: Y* ^# v2 y*/
void turn_R(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
* 读取 TFmini 测量结果
( V2 U- c% K1 w: |" r% k9 V*/
8 o9 b. _+ E6 ^5 cvoid getTFminiData(int* distance, int* strength){
static char i = 0;
4 I# M4 Z. h8 b4 ?! J; rchar j = 0;
- C1 {9 h* ^9 h& d+ ^/ V* Y# \$ l& Oint checksum = 0;
static int rx[9];
if(Serial.available()) {
rx = Serial.read();
' p* J& ^/ n* |& Wif(rx[0] != 0x59) {
& g5 ~, W0 z5 z/ ji = 0;
} else if(i == 1 && rx[1] != 0x59) {
i = 0;
+ u7 Q4 k& x9 F" a& }  ?} else if(i == 8) {
6 u+ v& ]- c* [for(j = 0; j < 8; j++) {
checksum += rx[j];
}
/*
; a& R4 o: e7 r" I- z9 c( R5 Hif(rx[8] == (checksum % 256)) {
& N! x2 t9 ^: s- s: C9 ?' \+ g*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;
( e. q  v% {. @* j} else {
3 Y& X$ y2 V  ]* V/ Mi++;
8 }( i$ C/ {7 F) G; k& [: R. a}
}
}
void setup() {
3 D8 n$ d3 t* T: L+ K// put your setup code here, to run once:
Serial.begin(115200);
//舵机的插口在 4
myservo.attach(4);
! l, E: J7 }/ o& Bbrake();
/*
% ?9 ]2 z. Q& D9 y! T* 将雷达指向前方
# T0 n$ `- z$ O- y/ z5 i. ]; \% ?0 @*/
1 h& E  ^! {, [# H  nmyservo.write(pos);
/*
* 设置轮胎电机输出口
*/
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
$ S; a; x6 C% N' vpinMode(6,OUTPUT);
3 U/ F* \5 D* I; f2 d& @pinMode(7,OUTPUT);
delay(10);
3 x. e+ _1 j2 h}
void loop() {
/*
* 读数一次
' p" X9 A4 n3 Z3 n*/
int distance = 0;
int strength = 0;
1 H& S( b; B) a% pgetTFminiData(&distance, &strength);
while(!distance) {
. |# ?! q6 A6 J$ B) kgetTFminiData(&distance, &strength);
- p+ G+ h! R9 H  H+ p; O; l  |Serial.print("Distance: ");
- D( t  f5 d) a' s% x0 n6 h, fSerial.print(distance);
Serial.print("cm ");
Serial.print("strength: ");
$ m8 j# a' s# {: L2 u6 E0 n9 [Serial.println(strength);
}
$ K: \% Z9 J- |! q: e6 Y/*
- \* Y2 j5 n  e. P) C6 B: \* 设置 30CM 阈值
7 s- n0 Y. e3 G( ?6 _0 G" K*/
if(distance <= 30 && distance > 0){
; k1 i/ ]; s9 z. M$ R; K/ w8 J: Ztemp_distance = distance;
  b4 s4 g& R$ e8 {}
delay(10);
8 \( Y8 d5 |  S1 E% S/*
* 判断读数距离
$ X8 p  Y5 G. k0 @5 E0 u0 u6 f2 q* 如果度数距离小于阈值，则停车，开始向左向右扫描，直到扫描出有空隙可以走，然后车轮转弯，然后扫描器回正
  H" }' a% C9 g# [. N* 如果读数距离大于阈值，则开车
$ {4 o6 {; X: J. g3 S' P9 n*/
if(temp_distance <= 30 && temp_distance >= 0){
0 }* s1 l4 K; k( y$ r- d1 Q! H. Xbrake();
$ d2 B7 a# h& a/ @* Y, k& A& R, o/*
. }+ F" j: K$ L$ s* v- T7 ~; t* 判断当前舵机应该向左还是向右转
*/
. o0 H  a* A$ N- Oif(flag){
/ n/ h2 q- f+ h4 [$ P7 ^- Kif(pos<170){
pos=pos+45;
}else{
- N% m2 B$ U' j3 A1 d& sflag = false;
}
/*
* 如果探测距离大于阈值，则舵机回正，小车转向
- C* }' p5 r2 c0 x8 J% y8 G) e*/
( o. `; }! o9 L2 u1 z5 `/ mif(distance > 32){
pos = 90;
myservo.write(pos);
! M# Z; Z% P7 Y  E% Zdelay(1200);
1 G8 u0 u0 Q$ G# G4 ^3 r9 C//判断小车回正方向
* o& V) f+ e# {if(pos >= 90){
% {0 h7 ~2 i; }1 @" q+ Y  ?5 c; l% bturn_L(35,35);
$ {$ |$ s5 ^: \0 U3 ]0 o/ j}else{
turn_R(35,35);
/ i; W/ \+ K( ~8 D}
, I/ y' ]4 m- Cdelay(250);
' c8 I& n: Y1 F5 \temp_distance = distance;
; ~3 p* D* U  u! H}
0 d+ X2 f9 _. `8 j. I7 j/*
* 如果探测距离小于阈值，则继续扫描
, o3 p1 T2 k9 d*/
4 X; y; s, \" W. B0 |' p: ~2 H& uelse{
' f: H4 ?7 ~& r: k' Z+ ?myservo.write(pos);
" i; R/ c* D8 Tdelay(1200);
4 w2 n) m. t5 ]" c9 s3 A}
8 G7 d6 a  H. Z: p1 K+ m. i}else{
) G8 i/ c! w8 \% {7 Hif(pos>10){
) C' _! F8 r6 Q8 F& opos=pos-45;
0 l/ @6 s& B9 m" E}else{
: U2 Q& v% n3 h, s9 G* l. Pflag=true;
* o4 {! q' i9 I5 e& S}
  j7 a: f* ?' Y/ K& [. l! o' T# {' N/*
* 如果探测距离大于阈值，则舵机回正，小车转向
" E" }; i+ j1 s& h+ C*/
if(distance > 32){
; w$ Y) B6 a6 i, ^( Y- P4 g3 U3 qpos = 90;
/ x6 s* i$ `' B: rmyservo.write(pos);
8 V1 n3 x0 o  K4 h* n% X! Ydelay(1200);
//判断小车回正方向
/ x  R  \: F; ?if(pos >= 90){
turn_L(35,35);
9 ~' \0 M, \) d" Z8 X* t}else{
turn_R(35,35);
4 V. {: D3 |7 [  U" w' X8 a}
( [* g: O) e+ L. l- v' ?2 Tdelay(250);
temp_distance = distance;
}
/*
& W" L  H& J1 _% s* 如果探测距离小于阈值，则继续扫描
0 q3 @/ e. T* D*/
9 h  G# F% j( `+ ?* D* `else{
% X' H9 \; ]8 |* R& c. D( `4 F: qmyservo.write(pos);
: G2 J: p$ w# p9 ^  C/ J. ?delay(1200);
) n+ v( c5 H" T7 A; k. B  Z7 c/ n}
& p) E6 W2 X* M8 p1 z, e6 K* V}
}
/*
/ N/ m/ ~% a  _2 @5 _9 E: C* 如果前方没有障碍物，直行
, s& D* Z' `) ~*/
1 A6 Z) b  ^1 n0 Q  Z  welse if(distance > 32 && distance < 1200){
advance(35,35);
}
/*
* 如果雷达挂了，小车停止
4 N; B! V: j! X' u*/
else if(distance == -3){
brake();
}
}

* Y$ S) N% a- l- m
0 f; e% C' N! @

jacky401

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