机器谱
图文展示3264(1)

图文展示3264(1)

副标题

概述
视觉循迹
躲避悬崖
双灰度循迹
三灰度循迹

2.结构说明

  该样机由两个 直流驱动轮模组 构成，驱动轮模组呈轴对称分布在车架上，再使用万向轮对车尾进行支撑，保持车身水平。

1.运动功能说明

    023号双轮底盘可以通过两个驱动轮的 差速运动 来实现前进、后退、原地转向、大半径转向等基本行驶功能。


前进与后退                                                           原地转向                                                             大半径转向

3.运动功能实现


3.1 电子硬件

在这个示例中，我们采用了以下硬件，请大家参考：

3.2 编写程序

前进功能的代码（点击查看：Forward.ino）

后退功能的代码（点击查看：Backward.ino）

主控板Basra（兼容Arduino Uno）
扩展板Bigfish2.1
电池7.4V锂电池


资料清单

序号

内容
1
样机3D文件
2
例程源代码
点击打包下载


将TT马达接在两个直流电机接口上，两个直流电机接口的针脚号分别为（D5,D6）以及（D9,D10），并将主控板和电池在车身固定好。

编程环境

Arduino IDE

语言

C语言

驱动部件

直流电机


4.扩展样机

   本样机也有一些扩展，如使用多个万向轮做支撑的版本，如下图所示。

5.资料下载

原地转向功能的代码（点击查看：TurnInPlace.ino）

大半径转向功能的代码（点击查看：BigTurn.ino）

概述
视觉循迹

1. 任务描述

      在机器人小车（R023d）上搭载摄像头，摄像头采集图像信息并通过WiFi将信息传递给PC端，然后PC端使用OpenCV对摄像头读取到的视频进行灰度化、高斯滤波、腐蚀、膨胀等处理，使图像分为黑白两色。PC端进行图像信息处理并将处理结果传递为下位机，下位机接收上位机处理的图像信息结果后便会控制小车相应运动，小车运动包含前进、左转、右转、停止。

2. 电子硬件

在这个示例中，我们采用了以下硬件，请大家参考：

主控板Basra（兼容Arduino Uno）
扩展板Bigfish2.1
电池7.4V锂电池

通信

2510通信转接板
WiFi路由器
其它摄像头x1、计算机x1


/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-02 https://www.robotway.com/

  -----------------------------------------------------------------------------------

/*

  wift car:

  2019/08/19:

  JN

  left: 9, 5;

  right: 10, 6;  

*/


const String FORWARD = "F";

const String BACK = "B";

const String LEFT = "L";

const String RIGHT = "R";

const String STOP = "S";


int speed_left = 41;

int speed_right = 41;


void setup() {

  Serial.begin(9600);

  pinMode(5, OUTPUT);

  pinMode(6, OUTPUT);

 

 

 

  pinMode(9, OUTPUT);

  pinMode(10, OUTPUT);

  Stop();

  delay(1000);

}


void loop() {

  String data = SerialRead();

 

  //if(data != ""){   

    if(data == FORWARD)

      Forward();

    else if(data == BACK)

      Back();

    else if(data == LEFT)

      Left();

    else if(data == RIGHT)

      Right();   

    else if(data == STOP)

      Stop();

// }

}


String SerialRead(){

  String str;

  while(Serial.available()){

    str += char(Serial.read());

  }

  return str;

}


void Forward(){

  analogWrite(9, speed_left);

  analogWrite(5, 0);

  analogWrite(6, 0);

  analogWrite(10, speed_right);

}


void Back(){

  analogWrite(9, 0);

  analogWrite(5, speed_left);

  analogWrite(6, speed_right);

  analogWrite(10, 0);

}


void Left(){

  analogWrite(9, 0);

  analogWrite(5, speed_left);

  analogWrite(6, 0);

  analogWrite(10, speed_right);

}


void Right(){

  analogWrite(9, speed_left);

  analogWrite(5, 0);

  analogWrite(6, speed_right);

  analogWrite(10, 0);

}


void Stop(){

  analogWrite(9, speed_left);

  analogWrite(5, speed_left);

  analogWrite(6, speed_right);

  analogWrite(10,speed_right);

}

/*******************************************************************************************

版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-02 https://www.robotway.com/

---------------------------------------------------------------------------------------

using System;

using System.IO;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using System.Threading.Tasks;

using System.Windows;

using System.Windows.Controls;

using System.Windows.Data;

using System.Windows.Documents;

using System.Windows.Input;

using System.Windows.Media;

using System.Windows.Media.Imaging;

using System.Windows.Navigation;

using System.Windows.Shapes;

using System.Windows.Media.Animation;

using System.Threading;

using OpenCvSharp;

using System.Drawing;

using System.Drawing.Imaging;

using System.Net;

using System.Net.Sockets;


namespace Tracking_Car

{

    /// <summary>

    /// Tracking_Car

    /// </summary>

    public partial class MainWindow : System.Windows.Window

    {

        //定义视频，控制地址以及控制端口变量

        static string CameraIp = "http://192.168.8.1:8083/?action=stream";

        static string ControlIp = "192.168.8.1";

        static string Port = "2001";


        //定义上位机发送的控制命令变量

        //定义命令变量

        string CMD_FORWARD = "", CMD_TURN_LEFT = "", CMD_TURN_RIGHT = "", CMD_STOP = "";


        /*

         * 指针角度对应各颜色

         * 25 -> 红色

         * 90 -> 绿色

         * 150 -> 蓝色

         */

        int ANGLE_LEFT = 0;

        int ANGLE_GO = 0;

        int ANGLE_RIGHT = 0;


        //黑色像素在左右两侧所占比例

        double numOfleft = 0.0;

        double numOfright = 0.0;


        //创建视频图像实例

        VideoCapture capture = new VideoCapture(CameraIp); //图像大小：宽度 X 长度 = 160 X 120;

        Mat frame = new Mat();   //存储视频每一帧图像像素

        Mat result = new Mat(); //存储二值化图像


        static byte[] kernelValues = { 0, 1, 0, 1, 1, 1, 0, 1, 0 }; // cross (+)

        Mat kernel = new Mat(3, 3, MatType.CV_8UC1, kernelValues);


        //图像中心线坐标

        int x1, y1, x2, y2;

        //窗口面积

        float area;


        //视频显示切换变量

        Boolean isChange = false;

        //循迹开始开关变量

        Boolean isBegin = false;


        public MainWindow()

        {

            InitializeComponent();

        }


        private void Window_Loaded(object sender, RoutedEventArgs e)

        {

            Assignment();

        }


        //变量赋值函数

        private void Assignment()

        {

            ANGLE_LEFT = 25;

            ANGLE_GO = 90;

            ANGLE_RIGHT = 150;


            rateLeft.Height = 10;

            rateRight.Height = 10;


            x1 = 80;

            y1 = 0;

            x2 = x1;

            y2 = 120;

            area = 160 * 120 / 2;


            CMD_FORWARD = "F";

            CMD_TURN_LEFT = "L";

            CMD_TURN_RIGHT = "R";

            CMD_STOP = "S";

        }


        /// <summary>

        /// MatToBitmap(Mat image)

        /// </summary>

        public static Bitmap MatToBitmap(Mat image)

        {

            return OpenCvSharp.Extensions.BitmapConverter.ToBitmap(image);

        }


        /// <summary>

        /// BitmapToBitmapImage(System.Drawing.Bitmap bitmap)

        /// </summary>

        public static BitmapImage BitmapToBitmapImage(Bitmap bitmap)

        {

            using (MemoryStream stream = new MemoryStream())

            {

                bitmap.Save(stream, ImageFormat.Png); //格式选Bmp时，不带透明度


                stream.Position = 0;

                BitmapImage result = new BitmapImage();

                result.BeginInit();

                // According to MSDN, "The default OnDemand cache option retains access to the stream until the image is needed."

                // Force the bitmap to load right now so we can dispose the stream.

                result.CacheOption = BitmapCacheOption.OnLoad;

                result.StreamSource = stream;

                result.EndInit();

                result.Freeze();

                return result;

            }

        }


        //颜色指示动画函数

        int angelCurrent = 0;

        private void ColorIndicate(int where)

        {

            RotateTransform rt = new RotateTransform();

            rt.CenterX = 130;

            rt.CenterY = 200;


            this.indicatorPin.RenderTransform = rt;


            double timeAnimation = Math.Abs(angelCurrent - where) * 5;

            DoubleAnimation da = new DoubleAnimation(angelCurrent, where, new Duration(TimeSpan.FromMilliseconds(timeAnimation)));

            da.AccelerationRatio = 0.8;

            rt.BeginAnimation(RotateTransform.AngleProperty, da);


            switch (where)

            {

                case 25:

                    dirDisplay.Content = "左转";

                    break;

                case 90:

                    dirDisplay.Content = "前进";

                    break;

                case 150:

                    dirDisplay.Content = "右转";

                    break;

                default:

                    dirDisplay.Content = "方向指示";

                    break;

            }


            angelCurrent = where;

        }


        //检测函数

        private void ColorDetect() {

            //将摄像头RGB图像转化为灰度图，便于后续算法处理

            Mat gray = frame.CvtColor(ColorConversionCodes.BGR2GRAY);

            //进行高斯滤波

            Mat binary = gray.Threshold(0, 255, ThresholdTypes.Otsu | ThresholdTypes.Binary);

            //闭运算，先膨胀后腐蚀，消除小型黑洞

            Cv2.Dilate(binary, binary, null);

            Cv2.Erode(binary, binary, kernel);


            result = binary.Clone();

            result.Line(new OpenCvSharp.Point(x1, y1), new OpenCvSharp.Point(x2, y2), new Scalar(255, 255, 255), 1);


            float rateOfleft = 0, rateOfRight = 0;

            var indexer = result.GetGenericIndexer<Vec3b>();

            for (int i = 0; i < result.Rows; i++) {

                for (int j = 0; j < result.Cols; j++) {

                    int B = indexer[i, j][0];

                    int G = indexer[i, j][1];

                    int R = indexer[i, j][2];


                    if (B == 0 && G == 0 && R == 0) {

                        if (j <= x1) {

                            numOfleft++;

                        }

                        else

                        {

                            numOfright++;

                        }

                    }

                }

            }


            rateOfleft = (float)(numOfleft) / area * 100;

            rateOfRight = (float)(numOfright) / area * 100;

            rateLeft.Height = rateOfleft;

            rateRight.Height = rateOfRight;

            numOfleft = 0;

            numOfright = 0;

        }


        //命令发送函数

        void SendData(string data)

        {

            try

            {

                IPAddress ips = IPAddress.Parse(ControlIp.ToString());//("192.168.8.1");

                IPEndPoint ipe = new IPEndPoint(ips, Convert.ToInt32(Port.ToString()));//把ip和端口转化为IPEndPoint实例

                Socket c = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);//创建一个Socket


                c.Connect(ipe);//连接到服务器


                byte[] bs = Encoding.ASCII.GetBytes(data);

                c.Send(bs, bs.Length, 0);//发送测试信息

                c.Close();

            }

            catch (Exception e)

            {

                MessageBox.Show(e.Message);

            }

        }


        //方向指示更新及命令发送

        private void CommandSend() {

            double l = rateLeft.Height;

            double r = rateRight.Height;


            if (isBegin) {

                if (Math.Abs(l - r) < 20)   //两侧黑色轨迹基本相同，前进

                {

                    ColorIndicate(ANGLE_GO);

                    SendData(CMD_FORWARD);

                }

                else if ((l - r) < -50)     //左侧黑色轨迹小于右侧，右转

                {

                    ColorIndicate(ANGLE_RIGHT);

                    SendData(CMD_TURN_RIGHT);


                }

                else if ((l - r) > 50)      //右侧黑色轨迹小于左侧，左转

                {

                    ColorIndicate(ANGLE_LEFT);

                    SendData(CMD_TURN_LEFT);

                }

            }

        }


        //视频显示函数

        private void ThreadCapShow()

        {


            while (true)

            {

                try

                {

                    capture.Read(frame); // same as cvQueryFrame

                    if (frame.Empty())

                        break;


                    this.Dispatcher.Invoke(

                        new Action(

                            delegate

                            {

                                if (isChange)

                                {

                                    //检测图像左右两侧黑色像素所占的比例，并显示图像

                                    ColorDetect();

                                    originImage.Source = BitmapToBitmapImage(MatToBitmap(result));

                                    CommandSend();

                                    result = null;

                                }

                                else

                                {

                                    originImage.Source = BitmapToBitmapImage(MatToBitmap(frame));

                                }

                            }

                            ));

                    //Cv2.WaitKey(100);

                    //bitimg = null;

                }

                catch { }

            }

        }


        //加载视频

        private void loadBtn_Click(object sender, RoutedEventArgs e)

        {

            if (originImage.Source != null) return;

            Thread m_thread = new Thread(ThreadCapShow);

            m_thread.IsBackground = true;

            m_thread.Start();

        }


        //切换视频显示，显示检测结果

        private void changeBtn_Click(object sender, RoutedEventArgs e)

        {

            if (!isChange)

            {

                isChange = true;

                changeBtn.Content = "返回";

            }

            else

            {

                isChange = false;

                changeBtn.Content = "切换";


                //指针角度归零

                ColorIndicate(0);

                rateLeft.Height = 10;

                rateRight.Height = 10;

                result = null;

            }

        }



3. 功能实现

视觉小车巡黑线工作原理：

      (1) 摄像头采集图像信息；

      (2) 通过 WiFi 将图像信息传递给 PC 端（VS2015 配置的 OpenCV 环境）；

      (3) 在 PC 端使用 OpenCV 对摄像头读取到的视频进行灰度化、高斯滤波、腐蚀、膨胀等处理，使图像分为黑白两色，采用 RGB 颜色模型作为黑白颜色判断；

      (4) 将图像对称分成左右两半，分别判断左、右计算检测在显示的摄像范围内的黑色像素区域所占比例=黑色像素范围/显示的摄像范围；

      (5) 比较两侧黑色像素区域所占比例大小确定前进方向，如果左侧比例大于右侧，则小车左偏离，进行右转；

      (6) PC端进行图像信息处理，将处理结果传递为下位机，下位机控制小车进行相应的运动；

3.1硬件连接

接线说明：

      ① 将2510通信转接板连接到扩展板的扩展坞上面；

      ② 用3根母对母杜邦线将2510通信转接板与WiFi路由器连接起来，GND-GND、RX-RX、TX-TX；

      ③ 找到1根USB线，一端连接到2510通信转接板接口上，另一端连接到WiFi路由器USB接口上；

      ④ 将摄像头线连接到WiFi路由器接口上。

3.2示例程序

编程环境：Arduino 1.8.19

① 下位机例程：

      下位机接收上位机处理的图像信息结果控制小车相应运动，小车运动包含前进、左转、右转、停止。

参考例程代码（car.ino）如下：

② 上位机例程：

     上位机（Visual Studio 2015.net下配置OpenCV环境）进行图像信息处理。下面提供一个参考例程（MainWindow.xaml.cs），大家可尝试根据实验效果改写。

4. 资料下载

资料清单

序号

内容
1

【R023】-视觉循迹-程序源代码

2【R023】-视觉循迹-样机3D文件
点击打包下载


        //循迹开始

        private void bgeinBtn_Click(object sender, RoutedEventArgs e)

        {

            isBegin = true;

        }


        //循迹停止

        private void stopBtn_Click(object sender, RoutedEventArgs e)

        {

            isBegin = false;

            SendData(CMD_STOP);

        }


    }

}

躲避悬崖

1. 功能说明

   本实验使用的样机为R023样机小型双轮差速底盘。在样机前方安装3个 近红外传感器 ，实现机器人躲避悬崖、在某平台上移动时不会掉下去的效果。

2. 电子硬件

   在这个示例中，我们采用了以下硬件，请大家参考：

主控板Basra（兼容Arduino Uno）
扩展板Bigfish2.1
传感器近红外传感器
电池7.4V锂电池


/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-10 https://www.robotway.com/                               

------------------------------------------------------------------------------------*/

void Right();

void Left();

void Stop();

void Forward();

void Back();


void setup()

{

  pinMode( 17, INPUT);

  pinMode( 18, INPUT);

  pinMode( 14, INPUT);

  pinMode( 10, OUTPUT);

  pinMode( 6, OUTPUT);

  pinMode( 5, OUTPUT);

  pinMode( 9, OUTPUT);

}


void loop()

{

  if (((!( digitalRead(14)) && ! (digitalRead(17)) ) ))

  {

    Forward();

  }

  if (( digitalRead(14) ))

  {

    Left();

    delay( 300 );

  }

  if (( digitalRead(17) ))

  {

    Right();

    delay( 300 );

  }

  if (( digitalRead(18) ))

  {

    Back();

    delay( 1000 );

    analogWrite(5 , 100);

    analogWrite(6 , 0);

    analogWrite(9 , 0);

    analogWrite(10 , 100);

    delay( 1500 );

  }

}


void Right()

{

  analogWrite(5 , 0);

  analogWrite(6 , 0);

  analogWrite(9 , 100);

  analogWrite(10 , 0);

}


void Forward()

{

  analogWrite(5 , 100);

  analogWrite(6 , 0);

  analogWrite(9 , 100);

  analogWrite(10 , 0);

}


void Back()

{

  analogWrite(5 , 0);

  analogWrite(6 , 100);

  analogWrite(9 , 0);

  analogWrite(10 , 100);

}


void Left()

{

  analogWrite(5 , 100);

  analogWrite(6 , 0);

  analogWrite(9 , 0);

  analogWrite(10 , 0);

}


void Stop()

{

  analogWrite(5 , 0);

  analogWrite(6 , 0);

  analogWrite(9 , 0);

  analogWrite(10 , 0);

}

左轮直流电机连在D9,D10接口上；右轮直流电机连在D5,D6接口上；3个近红外传感器从左到右分别连在A0、A4、A3接口上。

3. 示例程序

编程环境：Arduino 1.8.19

编写并烧录以下程序（nine.ino），该程序将实现演示视频中的动作。

4. 扩展

      本实验采用3个近红外传感器，利用的是近红外传感器能够识别到距离较近的桌面，无法识别到距离较远的地面，因此桌面要距离地面远一些。

      本实验还可以使用灰度传感器或者白标传感器。利用的是灰度和白标在悬崖处极难收到反射回来的红外线的原理，因此相应的桌面必须是浅色，如果桌面也是深色，灰度和白标传感器就无法区分桌面和悬崖了。


5. 资料下载

资料清单

序号

内容
1

R023-躲避悬崖-例程源代码

点击打包下载


双灰度循迹

1. 功能说明

   在机器人车体上安装2个 灰度传感器 ，实现机器人按照下图所指定的路线进行导航运动，来模拟仓库物流机器人按指定路线行进的工作过程。

2. 使用样机

    本实验使用的样机为R023e样机。

3. 功能实现

3.1 电子硬件

      在这个示例中，我们采用了以下硬件，请大家参考：

主控板Basra（兼容Arduino Uno）
扩展板

SH-01外围电路扩展板（与Bigfish扩展板相似）‍

传感器
灰度传感器
电池7.4V锂电池


传感器1传感器2小车状态动作
0
1
小车左偏向右调整
1
0
小车右偏向左调整
1
1
到达终点停止
0
0
正常前进

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-09 https://www.robotway.com/                                   

------------------------------------------------------------------------------------*/

void turnleft_slow();

void forward();

void carstop();

void turnright_slow();


void setup()

{

  pinMode( 18, INPUT);

  pinMode( 14, INPUT);

  pinMode( 10, OUTPUT);

  pinMode( 6, OUTPUT);

  pinMode( 5, OUTPUT);

  pinMode( 9, OUTPUT);

}


void loop()

{

  if (( digitalRead(14) && digitalRead(18) ))

  {

    forward();

  }

  if (( !( digitalRead(14) ) && digitalRead(18) ))

  {

    turnleft_slow();

  }

  if (( digitalRead(14) && !( digitalRead(18) ) ))

  {

    turnright_slow();

  }

  if (( !( digitalRead(14) ) && !( digitalRead(18) ) ))

  {

    carstop();

    delay( 5000 );

  }

}


void turnright_slow()

{

  analogWrite(6 , 80);

  analogWrite(10 , 0);

  analogWrite(5 , 0);

  analogWrite(9 , 0);

}


void carstop()

{

  analogWrite(6 , 0);

  analogWrite(10 , 0);

  analogWrite(5 , 0);

  analogWrite(9 , 0);

}


void turnleft_slow()

{

  analogWrite(6 , 0);

  analogWrite(10 , 0);

  analogWrite(5 , 80);

  analogWrite(9 , 0);

}


void forward()

{

  analogWrite(6 , 80);

  analogWrite(10 , 0);

  analogWrite(5 , 80);

  analogWrite(9 , 0);

}

void stop();

void left();

void right();

void forwards();


void setup()

{

  pinMode( 18, INPUT);

  pinMode( 14, INPUT);

  pinMode( 10, OUTPUT);

  pinMode( 6, OUTPUT);

  pinMode( 5, OUTPUT);

  pinMode( 9, OUTPUT);

}


void loop()

{

  if (( !( digitalRead(14) ) && digitalRead(18) ))

  {

    right();

  }

  else

  {

    if (( digitalRead(14) && !( digitalRead(18) ) ))

    {

      left();

    }

    else

    {

      if (( !( digitalRead(14) ) && !( digitalRead(18) ) ))

      {

        stop();

      }

      else

      {

        forwards();

      }

    }

  }

}


void stop()

{

  analogWrite(5 , 0);

  analogWrite(9 , 0);

  analogWrite(6 , 0);

  analogWrite(10 , 0);

}


void right()

{

  analogWrite(5 , 150);

  analogWrite(9 , 0);

  analogWrite(6 , 0);

  analogWrite(10 , 150);

}


void forwards()

{

  analogWrite(5 , 150);

  analogWrite(9 , 0);

  analogWrite(6 , 150);

  analogWrite(10 , 0);

}


void left()

{

  analogWrite(5 , 0);

  analogWrite(9 , 150);

  analogWrite(6 , 150);

  analogWrite(10 , 0);

}

电路连接说明：

① 电机连在D6，D10及D5，D9接口上；

② 2个灰度传感器分别接在扩展板的传感器接口A0、A4上。

3.2 编写程序

     传感器触发情况、小车行驶状态、对应行为策略表：

① 根据实验内容，利用多分支结构设计出程序流程图。

机器人轨迹导航任务流程图


② 根据设计好的程序流程图进行编程，编写并烧录以下程序（blackline_4if.ino），该程序将实现演示视频中的动作。

编程环境：Arduino 1.8.19

也可以使用if…else嵌套写法(blackline_ifelse.ino)。

4. 资料下载

资料清单

序号

内容
1

【R023】-灰度循迹2-例程源代码

2【R023】-灰度循迹2-样机3D文件
点击打包下载


三灰度循迹

1. 功能说明

    在小型双轮差速底盘样机前方安装3个 灰度传感器 ，实现机器人沿下图所指定的跑道路线进行运动的效果。

2. 使用样机

    本实验使用的样机为R023样机。

3. 功能实现

3.1 电子硬件

     在这个示例中，我们采用了以下硬件，请大家参考：

主控板Basra（兼容Arduino Uno）
扩展板

Bigfish2.1扩展板

传感器灰度传感器
电池7.4V锂电池


如果

机器人的某几个传感器触发了；

机器人的某几个电机做个什么事；

做多久；

如果

机器人的另外某几个传感器触发了；

机器人的某几个电机做个什么事；

做多久；

如果

机器人的1号传感器触发了；

机器人的左侧电机顺时针转；

机器人的右侧电机逆时针转；

持续5秒；

如果

机器人的2号传感器触发了；

机器人的左侧电机逆时针转；

机器人的右侧电机顺时针转；

持续5秒；

否则

都不转

if {   Sensor(端口a,触发);//传感器触发时此句为真，否则为假 }

{

    Motor(L,顺);

    Motor(R,逆);

    Delay 5;

}

if {   Sensor(端口b,触发); }

{

    Motor(L,逆);

    Motor(R,顺);

    Delay 5;

}

else

{

    Motor(L,停);

    Motor(R,停);

}

状态序号传感器1
1
1
20
0
1
10
状态序号传感器1传感器2
1
11
2
10
3
01
400

新序号

传感器1

传感器2
000
101
210
311
传感器1传感器2传感器3序号小车状态动作
0
0
00
都没触发，可能是跑偏了后退，转向
0011小车左偏左轮逆时针转，向右调整
0102小车正中左轮逆时针转，右轮顺时针转，前进
0113在这个行进方向上不可能无
1004小车右偏右轮顺时针转，向左调整
1015在此跑道上不可能无
1106遇到转角

右轮顺时针转，左转

1117

在此跑道上不可能

无


状态序号传感器1传感器2伪码

1

1

1

if

{
    Sensor(1,1);
    Sensor(2,1);
    ……
}

2

1


0

if

{
    Sensor(1,1);
    Sensor(2,0);
    ……
}

3

0

1

if

{
    Sensor(1,0);
    Sensor(2,1);
    ……
}

4

0

0

else

  ……

switch(s)

{

case 1 : {动作1;}break;

case 2 : {动作2;}break;

case 3 : {动作3;}break;

case 4 : Act_Stop();break;

default:;break;

}

s=0;

for(i=0;i<2;i++) //因为此例中有2个传感器，i取2

{

s=s|(Servo(i+1，触发判断)<<i);//获得传感器值，移位，或运算

}

switch(s)

{

case 0x00 : {动作0;}break; //序号也可以写作16进制数值

case 0x01 : {动作1;}break;

case 0x02 : {动作2;}break;

case 0x03 : {动作3;}break;

default:;break;

}

s=0;

for(i=0;i<3;i++)

{

s=s|(Input(i+1,1)<<i);

}

switch(s)

{

case 0x00 : 停;break;

case 0x01 : {Motor(L,逆);Motor(R,停);}break;

case 0x02 : {Motor(L,逆);Motor(R,顺);}break;

case 0x04 : {Motor(L,停);Motor(R,顺);}break;

case 0x06 : {Motor(L,停);Motor(R,顺);}break;

default:;break;

}

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-09 https://www.robotway.com/                                   

------------------------------------------------------------------------------------*/

int pin[3] = {A0, A3, A4};           //按车头前进方向，从右至左定义，后面经过公式计算，会转化为从左至右的顺序

int s;

void setup()

{

  pinMode( 5 , OUTPUT);

  pinMode( 6 , OUTPUT);

  pinMode( 9 , OUTPUT);

  pinMode( 10 , OUTPUT);

}


void loop()

{

   s = 0;

        for(int i=0; i<3; i++)                 //循环获取三个传感器的值

        {

            s|= (!digitalRead(pin[i]) << i);   //经过左移运算和或运算后，按照A0、A3、A4的顺序产生一个三位2进制数值，表示3个传感器的组合触发状态

        }

        switch (s)

        {

          case 0x00: //三个均未触发

          back();

          Left();

          break;

          case 0x01: //右侧传感器触发，直线上摆动或遇到右转弯

          Right();

          break;

          case 0x02: //中间传感器触发，直线上直行

          Forwards();

          break;

          case 0x04: //左侧传感器触发，直线上摆动或遇到左转弯

          Left();

          break;

          case 0x06: //左侧两个触发，遇到左转弯

          Left();

          break;

          default:;break;

        }

  }

void Left()

{

digitalWrite( 5 , LOW );

digitalWrite( 6 , HIGH);

digitalWrite( 9 , HIGH );

digitalWrite( 10 , LOW );

}


void Right()

{

digitalWrite( 5 , HIGH );

digitalWrite( 6 , LOW );

digitalWrite( 9 , LOW );

digitalWrite( 10 , HIGH );

}


void Forwards()

{

digitalWrite( 5 , HIGH );

digitalWrite( 6 , LOW );

digitalWrite( 9 , HIGH );

digitalWrite( 10 , LOW );

}


void back()

{

digitalWrite( 5 , LOW );

digitalWrite( 6 , HIGH );

digitalWrite( 9 , LOW );

digitalWrite( 10 , HIGH );

}

传感器1传感器2


   图层1.jpg

二进制结果十进制结果
1
111
3
1010
2
0101
1
0000
0

电路连接说明：

① 左轮直流电机连在D9，D10接口上；

② 右轮直流电机连在D5，D6接口上；

③ 3个灰度传感器从左至右连接在A0，A4，A3端口上。

3.2 编程框架

      本实验的编程框架用到了有限状态机。有限状态机（Finite-state machine）简称FSM，表示有限个状态以及在这些状态之间的转移和动作等行为的数学模型。它把复杂的控制逻辑分解成有限个稳定状态，在每个状态上判断事件。由于有限状态机有有限个状态，因此可以在实际中实现。有限状态机可以广泛的应用于机器人多个传感器触发组合状态的判断，大大提高检测效率。

状态表

      机器人的传感器触发一般用条件判断来做。

      这时机器人程序的一般思路是：

所以我们总是要用到大量的 if 语句，比如双轮小车的某个功能：

用伪码写出来就是：

在只有一个传感器的情况下，我们假设这是个开关量传感器。那么我们可以得到一个状态表格：

这个传感器有两个状态。

而当有两个传感器时，则有四个状态。

如果我们用 if 语句写这四个状态，就显得比较长。

      在编程的时候，状态罗列的越全，机器人的bug就越少。但是随着传感器的增多，状态数量按2的N次幂增加，大量的if语句使执行效率变得很低，经常出现识别不灵的情况。我们需要换一种高效写法。

      多个确定数量的传感器的触发组合，符合有限状态机的概念，有限状态机一般是用Switch语句来实现。如：

      不难发现，这段语句实现的关键，就是识别出上页表中的1、2、3、4，四个状态序号。

      那么问题就来了：我们如何让机器人知道自己传感器的触发组合对应于1、2、3、4的哪个序号呢？

二进制状态表

      下面，我们把每组传感器返回值看成一个二进制数值。

结果我们发现了一种新的、可计算的编码方式：

      于是，只要我们知道了传感器们的触发状态，也就知道了序号；知道了序号，也就知道了传感器们的触发状态。用这个序号去写switch语句，再合适不过了。下面我们要做的是，用一种算法，让机器人能够返回自己接收到的传感器组合值的二进制数据。

算法精解

      我们可以使用以下算法来实现：

l 首先设置一个变量s，这个s，将存储传感器组的二进制状态序号。

l 我们还需要用到一个重要的运算符“<<”，这个运算符的意义是：左移

     如：1<<n，意思是1向左移动n位，空出来的数位用0填补。

     如：1<<1，结果就是10；1<<2，结果就是100；101<<1，结果就是1010

l 只要让机器人依次返回各个传感器的状态数值，最早获取的，移到最左；第二获得的，移到“倒数第二左”，……，以此类推。即可获得。

 

如两个传感器均触发：

先获得1号的数值（真）并左移0位，得

再获得2号的数值（真）并左移1位，得

两数值取“或”，即可得11

数学问题解决了，很容易就可以转化为程序语句：

于是switch语句可以写为：

策略表

下面我们以本实验中的“小型双轮差速底盘-3灰度循迹”程序为例，再来推导一遍。

传感器触发情况、小车行驶状态、对应行为策略表如下：

伪码如下：

       这段代码中的动作，完全由策略表分析获得，因此，当状态比较多时，用户要学会利用策略表进行分析，从而确定机器人的动作策略，而不是凭空想象。

3.3 编写程序

编程环境：Arduino 1.8.19

编写并烧录以下程序（Track_Car.ino），该程序将实现演示视频中的动作。

4. 资料下载

资料清单

序号

内容
1

【R023】-3灰度循迹-例程源代码

点击打包下载


【R023】小型双轮差速底盘
作者：机器谱
© 2022 机器时代（北京）科技有限公司  版权所有
机器谱——机器人共享方案网站
学习  开发  设计  应用