The DAO alternates between driving forwards and backwards every x seconds.
If there is an obstacle in range of one of its four ultrasonic sensors, DAO stops, waits till it is removed, and then proceeds driving along the predefined path.

This program was written due to the DAO being supposed to make an appearence at the “Wiener Forschungsfest 2013″. There it will have to interact with other traffic participants, notice them, and avoid crashs.

Download

 

Editor

 

#define DEBUG
 
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
 
namespace WindowsFormsApplication1
{
    public partial class Form1 : Form
    {
        Boolean port1_is_open = false, port2_is_open = false, programRunning = true;
        Int32 speed = 0, angle = 0, pos = 0;
        Int32 safetyDistanceBack = 90, safetyDistanceFront = 40;
        Int32 uss1, uss2, uss3, uss4;
        Int32 driveTime = 4000;
 
 
        public Form1()
        {
            InitializeComponent();
        }
 
        private void com_box_TextChanged(object sender, EventArgs e)
        {
            if (com_box.Text != "" && com_box2.Text != "")
                connect_button.Enabled = true;
            else
                connect_button.Enabled = false;
        }
 
        private void com_box2_TextChanged(object sender, EventArgs e)
        {
            if (com_box.Text != "" && com_box2.Text != "")
                connect_button.Enabled = true;
            else
                connect_button.Enabled = false;
        }
 
        private void start_button_TextChanged(object sender, EventArgs e)
        {
            if (start_button.Text == "Start")
                connect_button.Enabled = true;
            else
                connect_button.Enabled = false;
        }
 
        private void connect_button_TextChanged(object sender, EventArgs e)
        {
            if (connect_button.Text == "Disconnect")
            {
                start_button.Enabled = true;
                com_box.Enabled = false;
                com_box2.Enabled = false;
            }
            else
            {
                start_button.Enabled = false;
                com_box.Enabled = true;
                com_box2.Enabled = true;
            }
        }
 
        private void connect_button_Click(object sender, EventArgs e)
        {
            if (port1_is_open)
            {//Disconnect
                serialPort1.Close();
                serialPort2.Close();
                connect_button.Text = "Connect";
                port1_is_open = false;
                port2_is_open = false;
            }
            else
            {
                try
                {//Connect
                    serialPort1.BaudRate = 9600;
                    serialPort1.StopBits = System.IO.Ports.StopBits.One;
                    serialPort1.Parity = System.IO.Ports.Parity.Even;
                    serialPort1.DataBits = 7;
                    serialPort1.PortName = "Com" + com_box.Text;
                    serialPort1.Open();
                    connect_button.Text = "Disconnect";
                    port1_is_open = true;
 
                    serialPort2.BaudRate = 38400;
                    serialPort2.StopBits = System.IO.Ports.StopBits.Two;
                    serialPort2.PortName = "Com" + com_box2.Text;
                    serialPort2.ReadTimeout = 500;
                    serialPort2.Open();
                    port2_is_open = true;
                }
                catch (Exception Ex)
                {
                    MessageBox.Show(Ex.Message);
                }
            }
        }
 
        private void start_button_Click(object sender, EventArgs e)
        {
            Thread workerThread = new Thread(DoWork);
 
            if (timer_key_listener.Enabled)
            {//Stop
                timer_key_listener.Stop();
                start_button.Text = "Start";
                workerThread.Abort();
                speed = 0;
                driver_ctl(speed, angle);
            }
            else
            {//Start
                timer_key_listener.Start();
                start_button.Text = "Stop";
 
                workerThread.Start();
            }
        }
 
        public void DoWork()
        {
            while (programRunning)
            {
                if (pos < 0)
                {
                    speed = 20;
                    for (Int32 i = pos; i < driveTime * 0.5; i = i + 10)
                    {
                        Thread.Sleep(10);
                        pos = i;
                    }
                }
                else
                {
                    speed = -20;
                    for (Int32 i = pos; i > driveTime * -0.5; i = i - 10)
                    {
                        Thread.Sleep(10);
                        pos = i;
                    }
                }
            }
        }
 
        private void timer_key_listener_Tick(object sender, EventArgs e)
        {
            if (serialPort1.IsOpen)
            {
                if (uss1 < safetyDistanceFront || uss2 < safetyDistanceFront || uss3 < safetyDistanceFront || uss4 < safetyDistanceBack)
                {
                    driver_ctl(0, 0);
#if DEBUG
                    Console.WriteLine("STOP USS1||USS2||USS3||USS4");
                    Console.WriteLine(uss1.ToString());
                    Console.WriteLine(uss2.ToString());
                    Console.WriteLine(uss3.ToString());
                    Console.WriteLine(uss4.ToString());
#endif
                }
                else
                {
                    driver_ctl(speed, angle);
                }
            }
        }
 
        private void driver_ctl(int speed, int steer)
        {
            // Steering
            if (Math.Abs(steer) < 10)
            {
                if (steer > 0)
                {
                    serialPort1.WriteLine("!B0" + Math.Abs(steer) + "\r");
                }
                else
                    if (steer <= 0)
                    {
                        serialPort1.WriteLine("!b0" + Math.Abs(steer) + "\r");
                    }
            }
            else
            {
                if (steer > 0)
                {
                    serialPort1.WriteLine("!B" + Math.Abs(steer) + "\r");
                }
                else
                    if (steer <= 0)
                    {
                        serialPort1.WriteLine("!b" + Math.Abs(steer) + "\r");
                    }
            }
 
            // speed
            if (Math.Abs(speed) < 10)
            {
                if (speed <= 0)
                {
 
                    serialPort1.WriteLine("!A0" + Math.Abs(speed) + "\r");
                }
                else
                    if (speed > 0)
                    {
 
                        serialPort1.WriteLine("!a0" + Math.Abs(speed) + "\r");
                    }
            }
            else
            {
                if (speed <= 0)
                {
 
                    serialPort1.WriteLine("!A" + Math.Abs(speed) + "\r");
                }
                else
                    if (speed > 0)
                    {
 
                        serialPort1.WriteLine("!a" + Math.Abs(speed) + "\r");
                    }
            }
        }
 
        private int read_distance(int Sensor, byte cmd)
        {
            //byte cmd;
            byte addr_h, addr_m, addr_l;
            byte distance_h, distance_l;
            byte data = 0;
            byte checksum;
            int tmp_checksum;
            int distance;
 
            addr_l = (byte)Sensor;
            Sensor = Sensor >> 8;
            addr_m = (byte)Sensor;
            Sensor = Sensor >> 8;
            addr_h = (byte)Sensor;
 
            tmp_checksum = ~(cmd + addr_h + addr_l + addr_m + data);
            checksum = (byte)tmp_checksum;
 
            // Break for 1ms
            serialPort2.BreakState = true;
            System.Threading.Thread.Sleep(1);
            serialPort2.BreakState = false;
            serialPort2.Write(new byte[6] { cmd, addr_h, addr_m, addr_l, data, checksum }, 0, 6);
 
            if (cmd == 94) //if reading command is given
            {
 
                distance_h = (byte)serialPort2.ReadByte();
                distance_l = (byte)serialPort2.ReadByte();
                distance = distance_h;
                distance = distance << 8;
                distance = distance | distance_l;
            }
            else
            {
                distance = 0;
            }
            return distance;
        }
 
        private void USS_check_Tick(object sender, EventArgs e)
        {
            try
            {
 
                if (serialPort2.IsOpen)
                {
                    //backwards
                    read_distance(4643, 81);
                    System.Threading.Thread.Sleep(70);
                    uss4 = read_distance(4643, 94);
                    progressBar4.Value = uss4;
 
                    //middle
                    read_distance(4640, 81);
                    System.Threading.Thread.Sleep(70);
                    uss1 = read_distance(4640, 94);
                    progressBar2.Value = uss1;
 
                    //left
                    read_distance(4641, 81);
                    System.Threading.Thread.Sleep(70);
                    uss2 = read_distance(4641, 94);
                    progressBar3.Value = uss2;
 
                    //right
                    read_distance(4642, 81);
                    System.Threading.Thread.Sleep(70);
                    uss3 = read_distance(4642, 94);
                    progressBar1.Value = uss3;
                }
            }
            catch (Exception err)
            {
                driver_ctl(0, 0);
                Console.WriteLine(err);
            }
        }
 
        private void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (connect_button.Text == "Disconnect")
            {
                MessageBox.Show("Please disconnect before ending the Program!");
                e.Cancel = true;
            }
            programRunning = false;
        }
    }
}//EOF

One Response to “C# – Drive along a path & Stop if there is an obstacle”

  1. byaba says:

    Hello how to read from RS485 DL/T645-1997 prtocol

Leave a Reply