查看: 570|回复: 0

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

[复制链接]
  • TA的每日心情
    开心
    2019-7-17 09:26
  • 签到天数: 1 天

    [LV.1]初来乍到

    发表于 2019-7-17 09:31 | 显示全部楼层 |阅读模式
    1.试验设备及接线
    1.1实验设备  
    ·        MiniQ 桌面机器人底盘
          1.png
    - 底盘直径:122mm
    - 轮子直径:42mm
    - 底盘高度:15mm
    - 兼容 Arduino 标准板及 Romeo 控制器固定孔
    - 电机参数:
    ·           N20 电机电压:3-9V
    ·          无负载转速:13000rpm
    ·          50:1 减速箱
    ·          260rpm@6V
    ·          40mA@6V
    ·          360mA 堵转@6V
    ·          10 盎司英寸扭矩@6V
                                                          
    ·        Romeo 三合一 Arduino 兼容控制器
    2.png
    -
    采用 Atmel Atmega328 单片机
    - Arduino UNO bootloader
    -
    完全兼容 Aruduino UNO 的端口布局
    -
    集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
    -
    支持 5 组 I2C 总线接口
    -
    支持两路电机驱动,峰值电流 2A,4 个控制口使用跳线切换
    -
    外部输入电压范围:6V~20V
    -
    更详细的参数介绍详见附录的网页地址。

    ·        MiniQ 小车上层安装板
    3.png
    ·        Benewake TFmini 标版
    4.png
    TFmini
    详细参数见 TFmini 使用说明。

    ·        9g 舵机
    5.png
    1.2接线
    6.png
    2.小车避障原理
    小车启动后,小车开始向前运动。当雷达探测到前方阈值内有障碍物时,小车停止运动,开始左右扫描寻路。舵机搭载 TFmini 从 90°开始向 180°扫描,然后从 180°向 0°扫描。
    7.png
    当扫描方向无障碍物时,小车向此方向转向,舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线,则小车后退,舵机回正。
    逻辑流程图如下所示:
    8.png
    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 == 8) {
    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();
    }
    }
    如您认可上述经验,欢迎您的联系:
    010-57456983
    www.benewake.com
    北醒(北京)光子科技有限公司
    北醒激光雷达
    电话:010-5745 6983
    邮箱:bw@benewake.com
    官方微信公众号:benewake-bj
    或搜索淘宝:北醒光子科技有限公司https://shop155404578.taobao.com/

    您需要登录后才可以回帖 登录 | 立即注册  

    本版积分规则

    热门推荐

    同时读取两个传感器的数据,并上传至云端存储
    同时读取两个传感器的数据
    一个示例,仅供参考,可以自己更换传感器,或者连接更多传感器。 开发板: Wifiduin
    求助帖 新手
    求助帖 新手
    如图,第一个LED灯驱动失败,求原因… 板子是UNO R3,LED灯接在13与GND上面。
    【Arduino】108种传感器系列实验(93)---CSR8350蓝牙音乐模块
    【Arduino】108种传感器系
    37款传感器与模块的提法,在网络上广泛流传,其实Arduino能够兼容的传感器模块肯定是
    光线跟踪钛合金猫眼
    光线跟踪钛合金猫眼
    +视频 https://www.bilibili.com/video/av67732712/ +原理 ---光线跟踪 建立极
    "高精度"米家电子温湿度计Pro拆解
    "高精度"米家电子温湿度计
    为了了解哪种温湿度传感器更好,我搞了一个米家电子温湿度计Pro(阿蒙同志赠送) 价格
    Copyright   ©2015-2016  Arduino中文社区  Powered by©Discuz!   ( 蜀ICP备14017632号-3 )
    快速回复 返回顶部 返回列表