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

北醒

7 a. {! R5 B# T8 K5 P8 _4 `
, O3 ~5 I4 I  ]: n5 B

1.试验设备及接线
- d: b# i5 t' Q0 M# _1.1实验设备  

·        MiniQ 桌面机器人底盘
   
- 底盘直径：122mm
" [; K8 H  J. q( a- 轮子直径：42mm
- 底盘高度：15mm
- 兼容 Arduino 标准板及 Romeo 控制器固定孔
- `; c* w) X* n/ k2 l$ |4 X2 Y" V7 C- 电机参数：

·           N20 电机电压：3-9V

·          无负载转速：13000rpm

·          50：1 减速箱

·          260rpm@6V

·          40mA@6V

·          360mA 堵转@6V

·          10 盎司英寸扭矩@6V

                                                      

·        Romeo 三合一 Arduino 兼容控制器

% U  C8 O0 u- v5 D7 A0 m# M- 采用 Atmel Atmega328 单片机
- Arduino UNO bootloader
- 完全兼容 Aruduino UNO 的端口布局
/ n0 C# @# i+ w+ |- 集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
- 支持 5 组 I2C 总线接口
6 y6 u2 X/ ]" y( ^$ E- 支持两路电机驱动，峰值电流 2A，4 个控制口使用跳线切换
" _4 L: g( I3 l2 k3 Z/ W3 Q- 外部输入电压范围：6V~20V
8 e- L$ v$ N! a9 J" K- 更详细的参数介绍详见附录的网页地址。

·        MiniQ 小车上层安装板

·        Benewake TFmini 标版

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

·        9g 舵机

1.2接线

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

4 m0 z; b- T! c6 T当扫描方向无障碍物时，小车向此方向转向，舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线，则小车后退，舵机回正。
逻辑流程图如下所示：
6 F; B& L6 y9 L( H* ?% V
; y6 T4 t7 \* ]6 J  w7 X7 z# i- f3.注意事项

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

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

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

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

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

4.附录
, _) T1 w$ Z! S/ ?, R* j4.1代码
#include <Servo.h>
Servo myservo;
+ a/ t4 y4 V$ x9 L) V! `5 g& ~int pos=90; //定义舵机角度
bool flag=true;//定义舵机转向
" h6 g+ E& ^: Y1 n. j0 ^float dist_f;//定义foward 方向距离
float dist_s;//定义sideway 方向距离
int E1=5; //定义 M1 使能
# c, h" o& p7 X7 Aint E2=6; //定义 M2 使能
int M1=4; //定义 M1 控制
int M2=7; //定义 M2 控制
int temp_distance =0;
/**
* 双轮停止
5 i5 ^& h* W* K& ~; j( g*/
void brake(void){
digitalWrite(E1,LOW); //给 E1 低电平
: }5 p+ a. V, [/ o: BdigitalWrite(E2,LOW); //给 E2 低电平
3 C  T+ u( Y% n' `' ~' o' Z}
/**
* 双轮前进
*/
void advance(char a, char b){
; d: t0 I$ p3 K' canalogWrite(E1,a);
5 T' |4 i" \/ }4 QdigitalWrite(M1,LOW);
analogWrite(E2,b);
/ F' s9 q. g$ P, [6 Q0 k9 \+ }0 y! AdigitalWrite(M2,LOW);
8 g6 a7 ]; p% B}
/**
" F8 n. K1 t- i# a3 `9 K* C* 双轮后退
( o1 d; b. j- X! s! q$ q5 d*/
' f4 V7 K1 Z/ w; pvoid back(char a, char b){
! Y( e6 x* g2 @7 c( oanalogWrite(E1,a);
+ a! H0 i- J- K& x( VdigitalWrite(M1,HIGH);
+ G* T6 y) R9 f, z% ?' RanalogWrite(E2,b);
/ t+ x, u, F+ v5 W0 j, r  y6 @digitalWrite(M2,HIGH);
, |1 H/ F( n" y% n}
  j/ y2 T' a' j6 \( r" H/**
2 p% m1 l) n. |  L4 M* 左转
*/
; `- D3 O! D4 jvoid turn_L(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
: ^/ f$ j7 K, I- F! {digitalWrite(M2,HIGH);
+ F/ A. x$ A0 \+ m4 v! V" ]% h7 T}
/**
* 右转
/ u+ j6 z: o& n- O' D" w: \7 \*/
! g/ J3 C' y9 r+ w- W( mvoid turn_R(char a, char b){
analogWrite(E1,a);
2 u3 m( L- f$ j/ i$ YdigitalWrite(M1,HIGH);
analogWrite(E2,b);
% [6 m1 I+ f3 M% Q% ^& U' |digitalWrite(M2,LOW);
+ ]$ o! O7 Q- ^}
/**
$ P! B- s1 K' N) U* 读取 TFmini 测量结果
*/
void getTFminiData(int* distance, int* strength){
static char i = 0;
( m+ e4 O: T7 u+ Nchar j = 0;
0 X' b- P4 }) l: L9 h) l( `3 eint checksum = 0;
9 O* U+ }8 ]  T$ }2 S  f& Ostatic int rx[9];
) D( t4 I: I5 Yif(Serial.available()) {
4 [+ U! e* V# w  q9 \& Zrx = Serial.read();
if(rx[0] != 0x59) {
2 ~) Y% L# _9 W& W* m* ji = 0;
$ F9 U. a9 Q) j% d) q8 V. }; k5 t' o. n} else if(i == 1 && rx[1] != 0x59) {
i = 0;
} else if(i == 8) {
for(j = 0; j < 8; j++) {
; R0 m3 z' m* _. N( Q6 g$ |0 Q% Wchecksum += rx[j];
}
) d$ }" G6 a0 E6 w& z/*
if(rx[8] == (checksum % 256)) {
! P7 o. _7 m/ w  d, _*distance = rx[2] + rx[3] * 256;
7 i2 f8 g+ o' P( l*strength = rx[4] + rx[5] * 256;
( ~$ t4 j, W3 \" I6 K0 `1 A}*/
*distance = rx[2] + rx[3] * 256;
5 |2 D$ v  y% R, o6 ^* F*strength = rx[4] + rx[5] * 256;
i = 0;
8 M2 [% X6 O' U* l: @' a1 I* x} else {
i++;
}
}
7 m  P1 u3 \% o1 q0 T7 l% X}
; ~) w2 X  {; k  J) ]2 I" Lvoid setup() {
+ W5 d( j1 Q* `) }" [7 J8 c1 X. b9 S3 P// put your setup code here, to run once:
Serial.begin(115200);
//舵机的插口在 4
2 o0 O" C; I0 umyservo.attach(4);
$ ^' {# z) j$ X+ x7 E- m! |; c/ Xbrake();
' g) {0 A, \) x( W0 }+ l: ]4 f5 _/*
* 将雷达指向前方
/ r; t3 w1 k* b# ^*/
myservo.write(pos);
/*
% b) ]& ?* e# [+ m* 设置轮胎电机输出口
, N- g4 j: l6 r0 P% ~, J/ h' b' [7 n*/
: [, B% g* J* ApinMode(4,OUTPUT);
pinMode(5,OUTPUT);
: s5 J+ m3 J4 C8 _. P- BpinMode(6,OUTPUT);
pinMode(7,OUTPUT);
delay(10);
}
2 r0 G$ h2 P. Uvoid loop() {
7 Q% _& ~" e6 x+ a3 U( {9 ~/*
# `' b6 e; [) ^* g8 |3 d* 读数一次
*/
int distance = 0;
3 J& P1 h$ J4 K6 J5 b) ]int strength = 0;
getTFminiData(&distance, &strength);
while(!distance) {
3 S7 C4 n/ f- @2 \4 l( vgetTFminiData(&distance, &strength);
Serial.print("Distance: ");
$ S, I4 {8 S0 j+ `Serial.print(distance);
Serial.print("cm ");
Serial.print("strength: ");
. _# w: ~' k, n0 l$ A, D* XSerial.println(strength);
, E. a7 z/ q) Q( ~; i( b}
, M0 p9 c& ?6 _  {2 Q/*
( w. [: A; K& p, r: l* 设置 30CM 阈值
6 W4 g5 s+ A4 r*/
if(distance <= 30 && distance > 0){
temp_distance = distance;
}
delay(10);
/*
* 判断读数距离
* 如果度数距离小于阈值，则停车，开始向左向右扫描，直到扫描出有空隙可以走，然后车轮转弯，然后扫描器回正
* 如果读数距离大于阈值，则开车
*/
' O3 y* O; _" Q( m  X& Bif(temp_distance <= 30 && temp_distance >= 0){
4 O4 P- g% l2 |* \1 ?- X* Hbrake();
/*
6 J2 @- K( R* `2 `* 判断当前舵机应该向左还是向右转
( V" S0 G  z: g7 q*/
if(flag){
if(pos<170){
pos=pos+45;
}else{
+ T7 E' j& q9 q( f( Z/ V3 rflag = false;
}
/*
* 如果探测距离大于阈值，则舵机回正，小车转向
*/
1 X+ h9 K9 t3 f4 ^2 k1 ~! Lif(distance > 32){
+ B& ]- `9 y0 Y+ H5 V, f' Vpos = 90;
  a+ k* f, W7 M# L+ r1 ?5 }1 Cmyservo.write(pos);
, c" t& r# Y& odelay(1200);
. a! w% F) u" n) W1 B$ p1 d: T% I//判断小车回正方向
5 B3 ?& G  C. o& c; J  i- h: O. R6 Rif(pos >= 90){
turn_L(35,35);
& s3 i" B: g% a! S5 n}else{
7 T+ F# m; j; I8 I4 w) D5 d6 Eturn_R(35,35);
}
9 H3 I4 ?: W3 Tdelay(250);
temp_distance = distance;
}
/*
* 如果探测距离小于阈值，则继续扫描
, _4 s% n& R( @" K- }*/
else{
myservo.write(pos);
8 W0 |' K8 v: E& E9 D6 ^- t" |delay(1200);
}
}else{
if(pos>10){
pos=pos-45;
. ?9 i! m5 U  ]2 \$ P$ Q}else{
flag=true;
7 V0 ^+ ~! f6 H" c+ g0 L3 c: j9 o}
/*
: p% u4 s6 r# b* 如果探测距离大于阈值，则舵机回正，小车转向
*/
) Y8 e' D+ I$ P* @if(distance > 32){
! e, W* |, Y% o1 B3 y3 `pos = 90;
myservo.write(pos);
delay(1200);
//判断小车回正方向
" T1 q4 [) W6 B' ~9 Jif(pos >= 90){
8 n- K: ^- n0 G6 r1 Dturn_L(35,35);
8 y' R/ r3 f. `" @. b  z: B}else{
turn_R(35,35);
/ P; I; u* M$ y, N}
delay(250);
1 O# X- E, n0 Itemp_distance = distance;
}
/*
* 如果探测距离小于阈值，则继续扫描
5 f: M9 J3 F' `& O% a: X  o* N*/
7 B# W+ x' ]% n: ]else{
myservo.write(pos);
delay(1200);
. ?  {4 h2 s3 j7 j# H}
}
}
/*
* 如果前方没有障碍物，直行
0 |5 |' v0 o( s  a% r7 S+ S/ H* X*/
" [. m: M9 o% s+ R( t9 celse if(distance > 32 && distance < 1200){
1 c. `1 H. I; Z3 Y; badvance(35,35);
* j9 r; ^/ Z  t( v: ]& a}
/*
* 如果雷达挂了，小车停止
' T* s3 e" \0 r*/
else if(distance == -3){
brake();
}
) L: Y, U& y5 @8 W+ `}

  S% N* P$ K3 F5 W7 \

jacky401

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