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

北醒

9 g  y7 `) J& X6 Q+ H7 z4 b

1.试验设备及接线
1.1实验设备  

·        MiniQ 桌面机器人底盘
9 Z) E# ?) q5 A) E! i   
7 A3 J& ^1 d# P) d$ C1 G- 底盘直径：122mm
- 轮子直径：42mm
- 底盘高度：15mm
) C' T8 K9 ]4 L+ k3 e- k0 p* Z, M- 兼容 Arduino 标准板及 Romeo 控制器固定孔
- 电机参数：

·           N20 电机电压：3-9V

·          无负载转速：13000rpm

·          50：1 减速箱

·          260rpm@6V

·          40mA@6V

·          360mA 堵转@6V

·          10 盎司英寸扭矩@6V

                                                      

·        Romeo 三合一 Arduino 兼容控制器

) N' z6 [1 C! z2 n0 z) P" m* q- 采用 Atmel Atmega328 单片机
- Arduino UNO bootloader
- 完全兼容 Aruduino UNO 的端口布局
, {9 s! L% Y& B" t& P- 集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
8 ]# {3 h# d" U( H  M- 支持 5 组 I2C 总线接口
- 支持两路电机驱动，峰值电流 2A，4 个控制口使用跳线切换
- 外部输入电压范围：6V~20V
, w8 [8 K! @) L! o% m8 E: O8 E- 更详细的参数介绍详见附录的网页地址。

·        MiniQ 小车上层安装板

$ E+ A0 A# b3 g, ^4 R# L
$ L1 e; j9 V: E2 D- H

·        Benewake TFmini 标版

* f5 {1 @4 }4 K+ i6 hTFmini 详细参数见 TFmini 使用说明。
1 j- `1 G+ L" K& c& }2 r& [
. ~, p) L1 @" H; h

·        9g 舵机

1.2接线
4 j( {$ k* k0 ?! h8 }+ v
2.小车避障原理
小车启动后，小车开始向前运动。当雷达探测到前方阈值内有障碍物时，小车停止运动，开始左右扫描寻路。舵机搭载 TFmini 从 90°开始向 180°扫描，然后从 180°向 0°扫描。
9 s3 p) h* K: P3 B6 V
当扫描方向无障碍物时，小车向此方向转向，舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线，则小车后退，舵机回正。
逻辑流程图如下所示：
  U: B0 a2 m3 E' q+ x& ]) @$ o1 V% Q
3.注意事项

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

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

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

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

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

4.附录
4.1代码
#include <Servo.h>
  t1 s2 |) I" T; M9 SServo myservo;
) B& G  S' v! S, U0 t4 m+ cint pos=90; //定义舵机角度
, T+ Z2 J2 r1 o) G0 [% t; r; zbool flag=true;//定义舵机转向
/ O3 G$ H$ h5 rfloat dist_f;//定义foward 方向距离
, e& s2 _( ^) W5 ^float dist_s;//定义sideway 方向距离
# R  ?& c/ O5 J6 uint E1=5; //定义 M1 使能
4 @& l4 l: n+ d2 S9 Hint E2=6; //定义 M2 使能
: W: ?6 i- w# f+ ?int M1=4; //定义 M1 控制
int M2=7; //定义 M2 控制
% h$ W0 p& O9 B6 L  c2 Y7 D2 qint temp_distance =0;
/**
* 双轮停止
. @$ F5 R3 \. F9 p*/
- [/ R& r0 z- J5 l' bvoid brake(void){
! p% `* l, {2 GdigitalWrite(E1,LOW); //给 E1 低电平
/ W5 H+ F2 a+ _% F( a0 T& J( y4 Y& |digitalWrite(E2,LOW); //给 E2 低电平
& ^/ }# C7 t' O0 }8 n9 u}
/**
* 双轮前进
*/
6 q: {4 o4 r' j  V+ C- Z) {void advance(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
) U8 g& t1 C9 S- E# Z6 FanalogWrite(E2,b);
+ J2 W8 \  h  r/ R) d; i6 w& ?! X, CdigitalWrite(M2,LOW);
}
/**
* 双轮后退
*/
void back(char a, char b){
  k' F. t4 r/ w) nanalogWrite(E1,a);
! v. n0 b5 }5 F+ l, J6 QdigitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
7 h1 O! h- z0 B- v}
/**
* 左转
9 `, A, j' g1 D2 A% [$ f$ N*/
void turn_L(char a, char b){
analogWrite(E1,a);
3 P. s2 Z( U& y) c. v! H: fdigitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}
/**
* 右转
*/
$ h# t$ P2 c$ k% C8 n8 P* Kvoid turn_R(char a, char b){
  q# G/ F; x  ]7 FanalogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,LOW);
0 Y6 _" X' [8 Y- a" Y( I) J+ G}
( `. R* q# }) u/**
( S+ \$ {! l+ X- x. l( |& Z( c* 读取 TFmini 测量结果
6 X5 g' q! x: G8 _& Y5 K) s*/
  u0 x) q/ q9 D  X, j. z" fvoid getTFminiData(int* distance, int* strength){
static char i = 0;
0 |$ x& A8 u( J- K3 m0 T: ^char j = 0;
int checksum = 0;
$ s: j0 u) Z* [& `static int rx[9];
if(Serial.available()) {
' V: p7 c$ d6 _. Y3 Rrx = Serial.read();
if(rx[0] != 0x59) {
i = 0;
2 a, @7 p1 P  o4 q; J" C* l} else if(i == 1 && rx[1] != 0x59) {
; e$ d: }4 \$ Vi = 0;
* \. f8 ~' e. m2 m} else if(i == 8) {
for(j = 0; j < 8; j++) {
0 D3 O/ _- Q. w6 @# \7 Dchecksum += rx[j];
8 z( `: L2 a/ f1 T' h1 H5 A% b}
/*
if(rx[8] == (checksum % 256)) {
* X8 B0 ^9 l4 ~' m* \; [7 _( j' c*distance = rx[2] + rx[3] * 256;
) K6 U1 y+ e. @  k*strength = rx[4] + rx[5] * 256;
' U0 V; ]9 w: B; `}*/
*distance = rx[2] + rx[3] * 256;
+ l3 G# ?# h: \% n*strength = rx[4] + rx[5] * 256;
i = 0;
+ M2 |) j0 r( {" e0 r} else {
* O0 G$ c* Q- ~8 P; Zi++;
}
}
}
void setup() {
1 S# T" p5 f* _- s// put your setup code here, to run once:
5 K" |; M9 `" tSerial.begin(115200);
//舵机的插口在 4
+ O" q* R2 V4 B7 b) b. rmyservo.attach(4);
4 X+ O6 e7 V: }" X3 cbrake();
5 ~1 Y0 ]6 s" S/*
* 将雷达指向前方
*/
8 U7 s6 p& x, m  pmyservo.write(pos);
! D2 P, h, E- |) l/*
9 _/ p3 j  |2 t& j' _% c* 设置轮胎电机输出口
*/
pinMode(4,OUTPUT);
6 e" p" K$ O& H" r6 X' c/ D7 ^3 RpinMode(5,OUTPUT);
pinMode(6,OUTPUT);
% w# M% {% D- o3 z9 [4 xpinMode(7,OUTPUT);
% o' {( `3 @: idelay(10);
; R! \/ |3 v5 k6 N' k# Z" S}
void loop() {
/*
  L" H0 c/ Y! B- n& V1 s+ f* 读数一次
*/
int distance = 0;
int strength = 0;
getTFminiData(&distance, &strength);
! o5 E" {4 ^% I) `% kwhile(!distance) {
. V9 f7 f: `4 V/ j( ~' KgetTFminiData(&distance, &strength);
Serial.print("Distance: ");
5 V' q" O7 p: J, j) G! ASerial.print(distance);
Serial.print("cm ");
. h, e, I9 `3 G" Y6 kSerial.print("strength: ");
Serial.println(strength);
; S: d9 J7 \$ }* s0 ~3 i9 P7 q) k}
1 \0 l6 R$ j7 L. s: Y/*
* 设置 30CM 阈值
5 l% M3 }& n" F) V$ G; c*/
/ r, C: B. e; _if(distance <= 30 && distance > 0){
temp_distance = distance;
3 `/ |8 h  I  x# ?4 y}
; J! z( i7 |" p$ `3 n8 tdelay(10);
/*
& `: d. ^) K; n: s% A3 r* 判断读数距离
* 如果度数距离小于阈值，则停车，开始向左向右扫描，直到扫描出有空隙可以走，然后车轮转弯，然后扫描器回正
* 如果读数距离大于阈值，则开车
' D& n8 A) w" n8 ]5 L*/
if(temp_distance <= 30 && temp_distance >= 0){
brake();
/*
* 判断当前舵机应该向左还是向右转
*/
if(flag){
6 n: K- U1 B8 l+ \5 ?: Fif(pos<170){
pos=pos+45;
: r, g5 m' c' b: V( I2 S9 u) L}else{
flag = false;
}
: c. \5 E) ~9 r+ i, S$ z, k/*
* 如果探测距离大于阈值，则舵机回正，小车转向
5 F2 V- S: V3 V( y*/
6 M1 w% B2 M+ N9 G# |# O0 ]if(distance > 32){
pos = 90;
6 E2 B% N$ ]6 m* U% K& @4 Omyservo.write(pos);
$ H- R2 I% e$ c6 Hdelay(1200);
/ ^4 [6 y1 D. W6 T- J0 g- F//判断小车回正方向
% r  @  k1 Y, a3 U7 p% G9 hif(pos >= 90){
- q% `; Y. T9 zturn_L(35,35);
, F( I, ?& O2 L9 ]6 |6 M7 |}else{
turn_R(35,35);
}
delay(250);
temp_distance = distance;
}
% @/ R1 O8 ?. X/ }7 X3 R2 M/*
* 如果探测距离小于阈值，则继续扫描
*/
* O9 C7 x  a- q# l8 j/ melse{
myservo.write(pos);
7 w( h# _3 j1 @& H3 o$ Q8 |* odelay(1200);
; ~! D+ T; `4 a/ R. r}
+ u% g; z) U7 y}else{
. G/ W+ T3 Y6 c; d3 O0 Iif(pos>10){
pos=pos-45;
2 v7 S0 s( E% J$ |6 n}else{
3 x) l* q; q, z) I/ T: wflag=true;
}
/*
0 J) ]1 N+ c  E5 t" V# {* 如果探测距离大于阈值，则舵机回正，小车转向
*/
if(distance > 32){
pos = 90;
) g# ~7 @. s* M+ r5 d$ e  P$ Pmyservo.write(pos);
delay(1200);
//判断小车回正方向
; k0 U6 g! k9 d$ Lif(pos >= 90){
turn_L(35,35);
6 _' E2 _( \* R  J}else{
turn_R(35,35);
}
delay(250);
temp_distance = distance;
( `$ n0 E: p4 o}
; Q, F! K6 ^2 S$ e; O! z/*
2 d3 `0 D& U% D# N1 r* 如果探测距离小于阈值，则继续扫描
*/
else{
myservo.write(pos);
delay(1200);
}
}
5 j$ x$ H& P! [" G8 e, ?}
/*
( P: a9 ~7 Q( b1 }2 L* 如果前方没有障碍物，直行
4 @  k% [/ e+ u9 M*/
else if(distance > 32 && distance < 1200){
advance(35,35);
1 @" I1 S3 c* W% u! P4 t7 |}
/*
- H9 u/ D. J: P0 G1 M0 X# _* 如果雷达挂了，小车停止
5 o( t+ _8 k3 r2 u( F2 `/ \  h*/
- l, z8 Z0 j' e/ ]* I' Nelse if(distance == -3){
brake();
  J+ J* f5 _' c$ J}
" P* ]( H* |) q* {" [" v6 T3 v4 u}

7 a# J& c* b8 ]9 m( p

jacky401

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