4WD Robot

For this project I am going to have a go at building a basic robot, using a chassis kit, and Pi. I plan to use this as a learning exercise to do something a bit bigger in the future.

New Conversation

Join the discussion

Log in or create an account, and start talking!

Activity

Ollie made a post to 4WD Robot:

Compass Update

Compass Update

This will just be a quick update post, to let you know how I am getting on with adding the compass to the robot.

The integration of the LSM303D class that I created last time was fairly straight forward.  I simply added the class file to my Robot solution and then from the AutomonousControl::RefreshDecision() function, just under where I refresh the Ultra Sonic sensors at the top, I call the ReadHeading() function, then pass the Heading value to my Map drawing code.  I create the LSM303D device and initialise it in the AutomonousControl::Initialise() function.  

Once added, I gave it a go, but it became quickly apparently, that it was not working quite right.  The map seemed better that the timing method, but it was still not quite right  spinning the robot around gave a full 360 rotation, but it was not smooth all the way around.

I had read that the device would only give a good reading if it was flat, but it was possible to do some additional calculations that would compensate for the device not being flat.  This involves using the accelerometer to calculate the pitch and roll of the device and then also use the Z axis in combination with the X and Y of the magnetometer to calculate the heading.

Here is the updated ConvertToHeading function:

public void ConvertToHeading(short x, short y, short z, short ax, short ay, short az)
        {
            // Calculate the angle of the vector y,x
            double accXnorm = ax / Math.Sqrt(ax * ax + ay * ay + az * az);
            double accYnorm = ay / Math.Sqrt(ax * ax + ay * ay + az * az);

            // calculate pitch and roll
            double pitch = Math.Asin(accXnorm);
            double roll = -Math.Asin(accYnorm / Math.Cos(pitch));

            // calculate the heading
            double magXcomp = x * Math.Cos(pitch) + z * Math.Sin(pitch);
            double magYcomp = x * Math.Sin(roll) * Math.Sin(pitch) + y * Math.Cos(roll) - z * Math.Sin(roll) * Math.Cos(pitch);

            double heading = 180 * Math.Atan2(magYcomp, magXcomp) / Math.PI;

            // Normalize to 0-360
            if (heading < 0)
            {
                heading = 360 + heading;
            }

            Heading = heading;
        }

So after applying this code, it seems to give better reading when the magnetometer was not fully straight, but it still has a similar problem to the the original one.

So the next possibility is that the device needs to be calibrated, and this is what I will look at sorting out in my next post!

Here is the fully updated LSM303D class.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Windows.Devices.Enumeration;
using Windows.Devices.I2c;

namespace Robot
{
    public class LSM303D
    {
        public enum I2CAddress
        {
            Address0x1D = 0x1D,
            Address0x1E = 0x1E
        }

        private class LSM303DAddresses
        {
            public const byte SelfId = 0x49;

            // Control Registers
            public const byte ControlRegister0 = 0x1F; // General settings
            public const byte ControlRegister1 = 0x20; // Turns on accelerometer and configures data rate
            public const byte ControlRegister2 = 0x21; // Self test accelerometer, anti-aliasing accel filter
            public const byte ControlRegister3 = 0x22; // Interrupts
            public const byte ControlRegister4 = 0x23; // Interrupts
            public const byte ControlRegister5 = 0x24; // Turns on temperature sensor
            public const byte ControlRegister6 = 0x25; // Magnetic resolution selection, data rate config
            public const byte ControlRegister7 = 0x26; // Turns on magnetometer and adjusts mode

            // Magnetometer
            public const byte MagenetometerXLSB = 0x08;
            public const byte MagenetometerXMSB = 0x09;
            public const byte MagenetometerYLSB = 0x0A;
            public const byte MagenetometerYMSB = 0x0B;
            public const byte MagenetometerZLSB = 0x0C;
            public const byte MagenetometerZMSB = 0x0D;

            // Accelerometer
            public const byte AccelerometerXLSB = 0x28;
            public const byte AccelerometerXMSB = 0x29;
            public const byte AccelerometerYLSB = 0x2A;
            public const byte AccelerometerYMSB = 0x2B;
            public const byte AccelerometerZLSB = 0x2C;
            public const byte AccelerometerZMSB = 0x2D;

            // Temperature
            public const byte TemperatureMSB = 0x05;
            public const byte TemperatureLSB = 0x06;
        }

        public LSM303D()
        {

        }

        public async void Initialise(I2CAddress address)
        {
            bool success = false;

            try
            {
                // select the I2D device
                string deviceSelectorString = I2cDevice.GetDeviceSelector(); // get the string that allows the devices to be retrieved
                DeviceInformationCollection devices = await DeviceInformation.FindAllAsync(deviceSelectorString);

                if (devices.Count > 0)
                {
                    // create te i2c settings object
                    I2cConnectionSettings connectionSettings = new I2cConnectionSettings((int)address);
                    connectionSettings.BusSpeed = I2cBusSpeed.FastMode; // 400kHz

                    LSM303DDevice = await I2cDevice.FromIdAsync(devices[0].Id, connectionSettings);

                    if (LSM303DDevice != null)
                    {
                        byte readBuffer = 0x00;

                        // check that the device is there
                        if (ReadFromResgister(0x0F, out readBuffer) == true)
                        {
                            if (readBuffer == LSM303DAddresses.SelfId)
                            {
                                // Set up the device
                                success = true;
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister0, 0x00);
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister1, 0x57); // enable accelerometer, 50hz sampling
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister2, 0x00); // accelerometer to +/-16 gauss full scale
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister5, 0x64); // high resolution mode, theromoeter off, 6.25hz ODR
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister6, 0x20); // set +/- guass full scale
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister7, 0x00); // get magnetometer out of low power mode
                            }
                        }
                    }

                }
            }
            catch (Exception exception)
            {
                success = false;
            }
        }

        public bool ReadHeading()
        {
            bool success = false;
            byte lsb;
            byte msb;

            // read the values from the device
            ReadFromResgister(LSM303DAddresses.MagenetometerXLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.MagenetometerXMSB, out msb);
            short magetometerX = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.MagenetometerYLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.MagenetometerYMSB, out msb);
            short magetometerY = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.MagenetometerZLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.MagenetometerZMSB, out msb);
            short magetometerZ = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.AccelerometerXLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.AccelerometerXMSB, out msb);
            short accelX = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.AccelerometerYLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.AccelerometerYMSB, out msb);
            short accelY = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.AccelerometerZLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.AccelerometerZMSB, out msb);
            short accelZ = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ConvertToHeading(magetometerX, magetometerY, magetometerZ, accelX, accelY, accelZ );

            return success;
        }


        public void ConvertToHeading(short x, short y, short z, short ax, short ay, short az)
        {
            // Calculate the angle of the vector y,x
            double accXnorm = ax / Math.Sqrt(ax * ax + ay * ay + az * az);
            double accYnorm = ay / Math.Sqrt(ax * ax + ay * ay + az * az);

            // calculate pitch and roll
            double pitch = Math.Asin(accXnorm);
            double roll = -Math.Asin(accYnorm / Math.Cos(pitch));

            // calculate the heading
            double magXcomp = x * Math.Cos(pitch) + z * Math.Sin(pitch);
            double magYcomp = x * Math.Sin(roll) * Math.Sin(pitch) + y * Math.Cos(roll) - z * Math.Sin(roll) * Math.Cos(pitch);

            double heading = 180 * Math.Atan2(magYcomp, magXcomp) / Math.PI;

            // Normalize to 0-360
            if (heading < 0)
            {
                heading = 360 + heading;
            }

            Heading = heading;
        }


        private bool ReadFromResgister(byte registerByte, out byte readByte)
        {
            bool success = false;
            byte[] writeBuffer = new byte[1] { registerByte };
            byte[] readBuffer = new byte[1] { 0x00 };
            readByte = 0x00;

            try
            {
                LSM303DDevice.WriteRead(writeBuffer, readBuffer);
                readByte = readBuffer[0];
                success = true;
            }
            catch (Exception exception)
            {
                success = false;
            }

            return success;
        }

        private bool WriteToResgister(byte registerByte, byte writeByte)
        {
            bool success = false;
            //byte[] registerBuffer = new byte[1] { registerByte };
            byte[] writeBuffer = new byte[2] { registerByte, writeByte };

            try
            {

                LSM303DDevice.Write(writeBuffer);
                success = true;
            }
            catch (Exception exception)
            {
                success = false;
            }

            return success;
        }

        public double Heading
        {
            get;
            private set;
        }

        private I2cDevice LSM303DDevice;
    }
}

Comments

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

LSM303D Compass

LSM303D Compass

In my last post I added an LCD that showed a map of where the robot had been and where it had detected obstacles.  This basically updated the robots position by timing how long it was doing each manouver.  While I managed to get it closer than I thought I would, it was still far from perfect, and not really usable as a map.  The problem laid with the timing method, there are far too many variables that effect how quickly the robot moves and turns (surface grip, battery charge, processor scheduling, etc.).  So the plan is to use an alternative method to work out how far and in what direction the robot is travelling.

The first method that I am going to try is to use a LSM303D compass.  This is a relatively cheap magnetometer and accelerometer (I believe that it also has a temperature sensor).  The bit that I am interested in is the magnetometer, this measures the strength of the magnetic field in the x, y, z directions (3D space).  These values can be used to calculate a heading value, like you get on a compass.  My plan is to use the heading value to work out the direction the robot is travelling.

Before I integrate the compass into the robot code, I am going to do a simple standalone compass demo, to test the device and the code I will use to connect to it.

The LSM303D has a SPI and a I2C interface.  You may recall that I used a SPI interface in one of my other projects, the Christmas Lights, so this time I am going to have a go at I2C.  As with the SPI, Windows IoT has a I2C API/Driver that makes things easier and saves having to read the whole datasheet to look up the timings to send and receive each bit on the interface.

Here is the wiring I used to connect the device to the Raspberry Pi 2.

  • VDD and VIN need to be connected to the Pi's 3.3V
  • Ground (GND) is connected to the Pi's GND
  • SDA (I2C Data Pin) is connected to the Pi's I2C Data Pin (SDA1)
  • SCL (I2C Clock Pin) is connected to the Pi's I2C Clock Pin (SCL1)
  • CS Pin should be connected to the 3.3V, this tells the device to use I2C
  • SDO is set to GND, this tells the device to use 0x1E as its address.

I wired it on to a breadboard that I will mount on to the robot, though I may struggle as it is starting to get overloaded with components!

I created a blank Universal Windows project in visual studio 2015, and added the IoT extensions reference, see here for more information.

I will post the full code at the bottom of the post (I really need to sort out a github account...), but first lets have a look at how it works.  I will focus on the LSM303D class that I have created, there is not much to the GUI part so I won't go into that.

The first job is to setup the I2C API and get a reference to the device.

// select the I2D device
string deviceSelectorString = I2cDevice.GetDeviceSelector(); // get the string that allows the devices to be retrieved
DeviceInformationCollection devices = await DeviceInformation.FindAllAsync(deviceSelectorString);

I2cConnectionSettings connectionSettings = new I2cConnectionSettings((int)address);
connectionSettings.BusSpeed = I2cBusSpeed.FastMode; // 400kHz

LSM303DDevice = await I2cDevice.FromIdAsync(devices[0].Id, connectionSettings);

To do this, the above code first retrieves a list of all the available I2C devices, using the I2C device selector string.  Next the I2CConnectionSetting object is created, which we pass the address and then also set the bus speed to FastMode which is 400kHz.

We can then get a reference to the using the Id of the first device found (there will only be one), and the connection settings.

Once the connection is established we next need to set up the LSM303D to do what we want it to. This involves set the options in the control registers.  I have just used some standard setting that I found in some sample code provided by the manufacturer of the device.  If you wish to change anything then you can find the options in the LSM303D datasheet.  Here is the code I used:

byte readBuffer = 0x00;

// check that the device is there
if (ReadFromResgister(0x0F, out readBuffer) == true)
{
    if (readBuffer == LSM303DAddresses.SelfId)
    {
        // Set up the device
        success = true;
        success &= WriteToResgister(LSM303DAddresses.ControlRegister0, 0x00);
        success &= WriteToResgister(LSM303DAddresses.ControlRegister1, 0x57); // enable accelerometer, 50hz sampling
        success &= WriteToResgister(LSM303DAddresses.ControlRegister2, 0x00); // set +/- 2g full scale
        success &= WriteToResgister(LSM303DAddresses.ControlRegister5, 0x64); // high resolution mode, theromoeter off, 6.25hz ODR
        success &= WriteToResgister(LSM303DAddresses.ControlRegister6, 0x20); // set +/- guass full scale
        success &= WriteToResgister(LSM303DAddresses.ControlRegister7, 0x00); // get magnetometer out of low power mode
    }
}

Communicating with I2C devices basically involves reading and writing to registers on the device.  Each register is a location that holds a value in the case of this device each register is one byte and is addressed by a one byte address.  Some registers are read only other can be written to and read from (a good way to check your code and wiring is working is to write a value to a register and then read it back to make sure it is the same value, just make sure where you are writing to is not going to do something to the device that would make it difficult to read back i.e. putting it in sleep mode!).  The base I2C device class provide a basic read and write function (as well as a ReadWrite function), but I have wrapped these up to make them easier to use in my code.  I have created a ReadFromRegister and a WriteToRegister function (the code for these is in the full code at the bottom).

In the above the code, the first thing that I do after connecting to the device is to read from the Who Am I register (address 0x0F), this register should always return the same 'Self Id' value, so it is a good check to ensure that the device is behaving.

Next I set the values for each of the registers (note, I don't need to make any changes to register 3 and 4).

Once this is done, the Magentometer will start to take reading and store them in registers.  I next have a ReadHeading function that reads from these registers:

public bool ReadHeading()
{
    bool success = false;
    byte lsb;
    byte msb;

    // read the values from the device
    ReadFromResgister(LSM303DAddresses.MagenetometerXLSB, out lsb);
    ReadFromResgister(LSM303DAddresses.MagenetometerXMSB, out msb);
    short magetometerX = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

    ReadFromResgister(LSM303DAddresses.MagenetometerYLSB, out lsb);
    ReadFromResgister(LSM303DAddresses.MagenetometerYMSB, out msb);
    short magetometerY = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

    ReadFromResgister(LSM303DAddresses.MagenetometerZLSB, out lsb);
    ReadFromResgister(LSM303DAddresses.MagenetometerZMSB, out msb);
    short magetometerZ = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

    ConvertToHeading(magetometerX, magetometerY, magetometerZ);
  
    return success;
} 

As the raw Magnetometer values are greater than 255 (the maximum value that can be stored in a byte), the values are split across two bytes, a least significant byte (lsb) and a most significant byte (msb).  These can be joined together using the .Net BitConverter class.  We want to end up with a 16bit value (two bytes), which is signed (i.e. can have negative values), so we use the ToInt16() function, passing in the bytes in order from least significant to most significant.

Once I have the raw values for each of the axis I can then pass them to my ConverToHeading() function which does the maths to convert the raw values to a single heading value in degrees, where 0 degrees is North.  Here is the code:

public void ConvertToHeading(short x, short y, short z)
{
    // Calculate the angle of the vector y,x
    double heading = (Math.Atan2((double)x, (double)y) * 180) / Math.PI;

    // Normalize to 0-360
    if (heading < 0)
    {
        heading = 360 + heading;
    }

    Heading = heading;
}

  And that is it, at the end of the function I set a Heading Property that is used by the GUI class to display the heading on the screen.

Here is the complete code, first the LSM303D class:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Windows.Devices.Enumeration;
using Windows.Devices.I2c;

namespace Compass
{
    public class LSM303D
    {
        public enum I2CAddress
        {
            Address0x1D = 0x1D,
            Address0x1E = 0x1E
        }

        private class LSM303DAddresses
        {
            public const byte SelfId = 0x49;

            // Control Registers
            public const byte ControlRegister0 = 0x1F; // General settings
            public const byte ControlRegister1 = 0x20; // Turns on accelerometer and configures data rate
            public const byte ControlRegister2 = 0x21; // Self test accelerometer, anti-aliasing accel filter
            public const byte ControlRegister3 = 0x22; // Interrupts
            public const byte ControlRegister4 = 0x23; // Interrupts
            public const byte ControlRegister5 = 0x24; // Turns on temperature sensor
            public const byte ControlRegister6 = 0x25; // Magnetic resolution selection, data rate config
            public const byte ControlRegister7 = 0x26; // Turns on magnetometer and adjusts mode

            // Magnetometer
            public const byte MagenetometerXLSB = 0x08;
            public const byte MagenetometerXMSB = 0x09;
            public const byte MagenetometerYLSB = 0x0A;
            public const byte MagenetometerYMSB = 0x0B;
            public const byte MagenetometerZLSB = 0x0C;
            public const byte MagenetometerZMSB = 0x0D;

            // Accelerometer
            public const byte AccelerometerXLSB = 0x28;
            public const byte AccelerometerXMSB = 0x29;
            public const byte AccelerometerYLSB = 0x2A;
            public const byte AccelerometerYMSB = 0x2B;
            public const byte AccelerometerZLSB = 0x2C;
            public const byte AccelerometerZMSB = 0x2D;

            // Temperature
            public const byte TemperatureMSB = 0x05;
            public const byte TemperatureLSB = 0x06;
        }

        public LSM303D()
        {

        }

        public async void Initialise(I2CAddress address)
        {
            bool success = false;

            try
            {
                // select the I2D device
                string deviceSelectorString = I2cDevice.GetDeviceSelector(); // get the string that allows the devices to be retrieved
                DeviceInformationCollection devices = await DeviceInformation.FindAllAsync(deviceSelectorString);

                if (devices.Count > 0)
                {
                    // create te i2c settings object
                    I2cConnectionSettings connectionSettings = new I2cConnectionSettings((int)address);
                    connectionSettings.BusSpeed = I2cBusSpeed.FastMode; // 400kHz

                    LSM303DDevice = await I2cDevice.FromIdAsync(devices[0].Id, connectionSettings);

                    if (LSM303DDevice != null)
                    {                       
                        byte readBuffer = 0x00;

                        // check that the device is there
                        if (ReadFromResgister(0x0F, out readBuffer) == true)
                        {
                            if (readBuffer == LSM303DAddresses.SelfId)
                            {
                                // Set up the device
                                success = true;
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister0, 0x00);
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister1, 0x57); // enable accelerometer, 50hz sampling
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister2, 0x00); // set +/- 2g full scale
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister5, 0x64); // high resolution mode, theromoeter off, 6.25hz ODR
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister6, 0x20); // set +/- guass full scale
                                success &= WriteToResgister(LSM303DAddresses.ControlRegister7, 0x00); // get magnetometer out of low power mode
                            }
                        }
                    }

                }
            }
            catch(Exception exception)
            {
                success = false;
            }
        }

        public bool ReadHeading()
        {
            bool success = false;
            byte lsb;
            byte msb;

            // read the values from the device
            ReadFromResgister(LSM303DAddresses.MagenetometerXLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.MagenetometerXMSB, out msb);
            short magetometerX = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.MagenetometerYLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.MagenetometerYMSB, out msb);
            short magetometerY = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ReadFromResgister(LSM303DAddresses.MagenetometerZLSB, out lsb);
            ReadFromResgister(LSM303DAddresses.MagenetometerZMSB, out msb);
            short magetometerZ = BitConverter.ToInt16(new byte[2] { lsb, msb }, 0);

            ConvertToHeading(magetometerX, magetometerY, magetometerZ);
  
            return success;
        } 


        public void ConvertToHeading(short x, short y, short z)
        {
            // Calculate the angle of the vector y,x
            double heading = (Math.Atan2((double)x, (double)y) * 180) / Math.PI;

            // Normalize to 0-360
            if (heading < 0)
            {
                heading = 360 + heading;
            }

            Heading = heading;
        }


        private bool ReadFromResgister(byte registerByte, out byte readByte)
        {
            bool success = false;
            byte[] writeBuffer = new byte[1] { registerByte };
            byte[] readBuffer = new byte[1] { 0x00 };
            readByte = 0x00;

            try
            {
                LSM303DDevice.WriteRead(writeBuffer, readBuffer);
                readByte = readBuffer[0];
                success = true;
            }
            catch(Exception exception)
            {
                success = false; 
            }

            return success; 
        }

        private bool WriteToResgister(byte registerByte, byte writeByte)
        {
            bool success = false;
            //byte[] registerBuffer = new byte[1] { registerByte };
            byte[] writeBuffer = new byte[2] { registerByte, writeByte };

            try
            {

                LSM303DDevice.Write(writeBuffer);
                success = true;
            }
            catch (Exception exception)
            {
                success = false;
            }

            return success;
        }

        public double Heading
        {
            get;
            private set;
        }

        private I2cDevice LSM303DDevice;
    }
}

Next the GUI XAML code:

<Page
    x:Class="Compass.MainPage"
    xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
    xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
    xmlns:local="using:Compass"
    xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
    xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
    mc:Ignorable="d">

    <Grid Background="{ThemeResource ApplicationPageBackgroundThemeBrush}">
        <TextBlock x:Name="heading" HorizontalAlignment="Left" Margin="39,33,0,0" TextWrapping="Wrap" Text="TextBlock" VerticalAlignment="Top" FontSize="29.333"/>
    </Grid>
</Page>

And finally the GUI code:

using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices.WindowsRuntime;
using System.Threading.Tasks;
using Windows.Foundation;
using Windows.Foundation.Collections;
using Windows.UI.Xaml;
using Windows.UI.Xaml.Controls;
using Windows.UI.Xaml.Controls.Primitives;
using Windows.UI.Xaml.Data;
using Windows.UI.Xaml.Input;
using Windows.UI.Xaml.Media;
using Windows.UI.Xaml.Navigation;

// The Blank Page item template is documented at http://go.microsoft.com/fwlink/?LinkId=402352&clcid=0x409

namespace Compass
{
    /// <summary>
    /// An empty page that can be used on its own or navigated to within a Frame.
    /// </summary>
    public sealed partial class MainPage : Page
    {
        public MainPage()
        {
            this.InitializeComponent();


            device = new LSM303D();

            device.Initialise(LSM303D.I2CAddress.Address0x1E);

            updateTimer = new DispatcherTimer();
            updateTimer.Interval = TimeSpan.FromMilliseconds(500);
            updateTimer.Tick += UpdateCompassHeading;
            updateTimer.Start();

        }

        public void UpdateCompassHeading(object sender, object e)
        {
            device.ReadHeading();
            heading.Text = device.Heading.ToString();
        }

        LSM303D device;
        DispatcherTimer updateTimer;

    }
}

In my next post I will integrate this code into my robot project and use it to draw the map!

Comments

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

Mapping the way!

Mapping the Way!

In my last post I added three ultrasonic sensors to the robot, in an attempt to make it autonomous.  This was somewhat successful, and it was able to drive around and generally avoid most obstacles.  There are things that can be done to improve this, and other sensors types that I can use, but first I am going to look at building up a map of where the robot has been and marking where obstacles where detected.

To achieve this I have attached an LCD to the Raspberry Pi and placed it on the top of the robot.  It is connected via HDMI and powered via another USB cable from the Anker power charger (luckily this supports charging/powering of two devices simultaneously, I believe to a max of 4A).

As in previous posts I am running Windows 10 IoT on the Pi, which is connected via Serial comms to an Arduino, which is talking to the Motor Driver using PWM (please see my previous posts for more details).

The plan is that every time the robot moves forward in my autonomous control processing loop, I am going to call a function that will draw a line on the user interface in the current direction.  I set the current direction by calling a TurnLeft or TurnRight function for every iteration of the processing loop where the robot is turning.  This is all based on timings, which will give me problems, but there are ways around that with a bit more hardware.

The code works by drawing XAML shape objects on to a Grid canvas, which is effectively the whole screen.  I have placed a start WPF Ellipse where the line will be draw from, I did this in the XAML designer in Visual Studio, so it is defined in the XAML and not in the code.

The first part of the code is the moving forward functionality (Note, all of this code is in the MainPage.xmal.cs file and part of the MainPage class, I know, not ideal...):

        public void Initialise()
        {
            Point currentPosition = new Point(250, 150);
            robotTrack = new Polyline();
            
            robotTrack.Visibility = Visibility.Visible;
            robotTrack.StrokeThickness = 3;
            robotTrack.Stroke = new SolidColorBrush(Colors.Blue);

            robotTrack.Points.Add(new Point(250, 250));

            myGrid.Children.Add(robotTrack);
        }

        // -------------------------------------------------------------------------
        // Code to handle the robot moving forward and drawing the line

        private const int ScaleFactor = 1; //  allows all values to be adjusted
                                           // definitions for the distance in pixels each move should be
        private const float FowardDistance = 1;

        private Polyline robotTrack;  // The XMAL drawing object

        // This function draws the next block on to the line as the robot moves
        public void MoveForward()
        {
            AddLine(CalculateEndPoint(FowardDistance * ScaleFactor));
        }

        private Point CalculateEndPoint(float distance)
        {
            double angle = heading * Math.PI / 180; // Convert to RADs
            double startX = RobotTrackLastPoint.X;
            double startY = RobotTrackLastPoint.Y;
            double endX = startX + distance * Math.Sin(angle);
            double endY = startY - distance * Math.Cos(angle);

            return new Point(endX, endY);
        }

        private void AddLine(Point targetPosition)
        {
            robotTrack.Points.Add(targetPosition);
        }

        private Point RobotTrackLastPoint
        {
            get
            {
                return robotTrack.Points.Last();
            }
        }

As mentioned above, the MoveForward() function is called every time the robot takes a 'step' forward.  This calls my AddLine() function, passing into it the end point of the line, which is calculated by the CalculateEndPoint() function.  CalculateEndPoint() uses trigonometry to work out the screen canvas X and Y end points based on the current position and the passed in distance (which is effectively the length of the line on the screen).  The line is drawn based on the current Heading, this is effectively the direction the robot is pointing.

I am using a WPF PolyLine object, and AddLine() simply adds a Point object to its list of points.

The second part of the code is to keep track of the heading of the robot.  There are many much better methods to do this with hardware, but for now I am just using a timing method.

        // -------------------------------------------------------------------------
        // Code to handle the Robot turning

        // This is the amount to add to the heading each call to the 
        // relevant turn function.  This needs to be tuned to the indvidual
        // robot
        private const double LeftTurnRate = -3.1;
        private const double RightTurnRate = 3.1;

        private double heading = 0;  // degrees Note; 0 deg is to North.

        public void TurnLeft()
        {
            // add to the heading a fixed amount, based on the amount turned
            AddToHeading(LeftTurnRate);
        }

        public void TurnRight()
        {
            // add to the heading a fixed amount, based on the amount turned
            AddToHeading(RightTurnRate);
        }

        private void AddToHeading(double amountToAdd)
        {
            heading += amountToAdd;

            // ensure that we dont go over 360 degrees
            if (heading < 0)
            {
                heading += 360;
            }

            if (heading >= 360)
            {
                heading = heading % 360;
            }
        }

The TurnLeft() and TurnRight() functions are called for each iteration of the autonomous control processing loop that the the robot is turning. These functions call the AddToHeading function passing in a defined constant amount.  This constant is the amount in degrees the robot has turned in one iteration, this value needs to be tuned to the individual robot and probably the surface, battery life, etc., so this is what makes this timing method far from ideal.  AddToHeading() does just that, but it also ensures that the degrees wrap around if they go below 0, or above 360.

The final part is to draw a circle on the canvas, where the robot thinks it has found an obstruction:

        // -------------------------------------------------------------------------
        // Code that adds an obsticle (rectangle) in front of the current heading
        //
        private const float ObstacleDistance = 40;
        private const int ObstacleSize = 10; // Pixels

        public void AddObstacle()
        {
            Point ObstacleLocation = CalculateEndPoint(ObstacleDistance * ScaleFactor);

            // Draw a small Red Circle at the location
            Ellipse circle = new Ellipse();

            circle.Visibility = Visibility.Visible;
            circle.StrokeThickness = 1;
            circle.Stroke = new SolidColorBrush(Colors.Red);
            circle.Fill = new SolidColorBrush(Colors.LightPink);
            circle.Width = ObstacleSize;
            circle.Height = ObstacleSize;

            circle.HorizontalAlignment = HorizontalAlignment.Left;
            circle.VerticalAlignment = VerticalAlignment.Top;

            Thickness thickness = new Thickness(ObstacleLocation.X - (ObstacleSize / 2), ObstacleLocation.Y - (ObstacleSize / 2), 0, 0);
            circle.Margin = thickness;

            myGrid.Children.Add(circle);
        }

This works in a similar way to the MoveForward function, but it draws an Ellipse at the end point rather than a line to the end point.  This function is called by the autonomous control processing loop, every time the robot has detected an obstacle in front of it.

Here is the updated Autonomous control function: 

        public void RefreshDecision(object sender, object e)
        {
            frontUltraSonicSensor.RefreshStatus();
            leftUltraSonicSensor.RefreshStatus();
            rightUltraSonicSensor.RefreshStatus();

            if ( (leftUltraSonicSensor.RecommendedAction == RobotAction.Forward) &&
                (frontUltraSonicSensor.RecommendedAction == RobotAction.Forward) &&
                (rightUltraSonicSensor.RecommendedAction == RobotAction.Forward) )
            {
                currentState = RobotState.Forward;
            }
            else if (  currentState == RobotState.Forward )
            {
                // there is a new obstruction
                if ( rightUltraSonicSensor.RecommendedAction == RobotAction.AvoidObstruction )
                {
                    // go left
                    currentState = RobotState.AvoidObstructionLeft;
                }
                else
                {
                    currentState = RobotState.AvoidObstructionRight;
                }
            }
            else
            {
                // we are currently trying to avoid an obstruction, so keep doing what we are doing!
                // (and hope for the best!)
            }

            if (currentState == RobotState.Forward)
            {
                RecommendedAction = RobotAction.Forward;
                mainPage.MoveForward();
            }
            else if (currentState == RobotState.AvoidObstructionLeft)
            {
                RecommendedAction = RobotAction.Left;
                mainPage.TurnLeft();
                mainPage.AddObstacle();
            }
            else
            {
                RecommendedAction = RobotAction.Right;
                mainPage.TurnRight();
                mainPage.AddObstacle();
            }   

            // do what is recommended!
            motorControl.Move(RecommendedAction);

        } 

As you can see, I simply pass in a reference to the MainPage (not shown above), which I then call the functions the new functions based on the current state of the robot.

After a bit of trial and error I managed to tune the values mentioned above and get it working reasonably well, well better than I thought I would:

Here is a photo of the generated map:

The obstructions (in red) are not being ideally placed as it looks like the robot drives through them.  This is because I am drawing the obstruction straight in front of the robot's path, but as I have three sensors, some times it will be one of the side ones that has been triggered.  This problem could be solved by drawing the obstruction based on which sensors were triggered.  I also haven't quite got the forward distance right as the robot actually ended up close to where it started.

Using this timing method has allowed me to prove the concept.  I could improve it, by changing the interface to the Arduino to carry out steps of movement,rather than continuous movement.  These steps could be measured, and I could then synchronise the steps to the calls to the MoveFoward(), TurnLeft() and TurnRight() functions.  This is still far from ideal, the best option would be to add wheel encoders to the robot.  These basically measure the actual rotations of the wheels, which allows me to calculate the actual distance moved, which will make it far more reliable and accurate.  

I may also consider displaying the map on a Web Page served up by the HTTP Server that I have created on the Pi.

Comments

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

Ultrasonic!

Ultrasonic

Following on from my last post, I thought, what if instead of getting the robot to stop when the IR Obstacle sensor is triggered, I instead got it to turn left, would that be enough for it to be autonomous?  So I gave it a quick go, and to some extent it worked!  how ever there were limitations, partially due to the range of the IR sensor, but also due to its placement.  It would get stuck on walls, etc.  So I tried placing the one sensor on the right hand side, so I was always turning away from it, but this just meant that it got more stuck against things on the left of the robot.  It became apparent I would need more that one sensor on the front of the robot.

I only have the one IR Obstacle sensor, but I had order 5 ultrasonic sensors (it was cheaper that way...), so I figured instead of just testing the one ultrasonic sensor, I would try three.  One facing forward, one to the left and one to the right.

An ultrasonic sensor works by sending out a ultrasonic sound pulse, from a transmitted, which will then bounce off objects in front of it and be picked up by a receiver, a bit like radar but with an ultra-frequency sound.  By measuring the time it takes for the sound to be picked up by the receiver after being transmitted, you can work out the distance travelled by using distance = speed x time.  We know the speed (as it is the speed of sound).  To get the actual distance to the object, you need to divide the answer by 2, as the sound travels from the transmitter, bounces off the object and travels back again.

The ultrasonic sensor boards, have 4 pins.  +5v, Ground, a Trigger pin and an Echo pin.  The trigger pin it to trigger the transmitting by sending a 10 micro second Low-High-Low pulse.  The board will then set the Echo pin Low, and once it has received the bounced back sound it will set the Echo pin High.  In our code, using GPIO pins on the Raspberry Pi, we need to send the initial pulse, then start a timer, and wait for the Echo pin to go High, once it does we stop the timer, and can then do the maths to find the distance.

Here is the complete code for my Ultrasonic Sensor Class:

using System;
using System.Diagnostics;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Threading;
using Windows.Devices.Gpio;

namespace Robot
{
    public class UltraSonicSensor
    { 
        public UltraSonicSensor(int triggerPinIn, int echoPinIn)
        {
            RecommendedAction = RobotAction.Neutral;

            gpio = GpioController.GetDefault();

            triggerPin = gpio.OpenPin(triggerPinIn);
            echoPin = gpio.OpenPin(echoPinIn);

            triggerPin.SetDriveMode(GpioPinDriveMode.Output);
            echoPin.SetDriveMode(GpioPinDriveMode.Input);

            triggerPin.Write(GpioPinValue.Low);
        }


        public double MeasureDistance()
        {
            ManualResetEventSlim resetEvent = new ManualResetEventSlim(false);
            ushort timeoutDuration = 10; //msec - this is the max time to attempt the reading
            double time = 0;
            double distance = 100000;
            Stopwatch pingTimer = new Stopwatch();
            Stopwatch timeoutTimer = new Stopwatch();
            bool abort = false;

            // Send quick pulse to tell the trasnmitter to transmit
            triggerPin.Write(GpioPinValue.High);
            resetEvent.Wait(TimeSpan.FromMilliseconds(0.01));
            triggerPin.Write(GpioPinValue.Low);

            timeoutTimer.Start();

            // Wait for return pulse
            while ((echoPin.Read() == GpioPinValue.Low) && (abort == false))
            {
                //check the timeout period
                if (timeoutTimer.ElapsedMilliseconds > timeoutDuration)
                {
                    time = 100;
                    abort = true;
                }
            }

            pingTimer.Start();

            // Wait for pulse to end
            while ((echoPin.Read() == GpioPinValue.High) && (abort == false))
            {
                //check the timeout period
                if (timeoutTimer.ElapsedMilliseconds > timeoutDuration)
                {
                    time = 100;
                    abort = true;
                }
            }

            pingTimer.Stop();

            time = pingTimer.Elapsed.TotalSeconds;

            // 17000 is the speed of sound divided by 2, as the pulse has to go to the source and come back
            distance = time * 17000;

            return distance;
        }


        public void RefreshStatus()
        {
            // Get the distance
            double currentDistance;

            currentDistance = MeasureDistance();
            
            if (currentDistance <= 15.0)
            {
                // object detected in front of this sensor so recomend turn around
                RecommendedAction = RobotAction.AvoidObstruction;
            }
            else
            {
                // no object detected so move forward
                RecommendedAction = RobotAction.Forward;
            }
        }

        public RobotAction RecommendedAction
        {
            get;
            set;
        }

        private GpioPin triggerPin;
        private GpioPin echoPin;
        GpioController gpio;
    }
}

You will notice that I have added a timeout check, this is so the sensor does not get stuck in an infinite loop waiting for the sound to be received.  I have set my timeout quite short, this is because I want to check the distance quite frequently (every 50msecs), on 3 sensors.  It is important to do the sensor one at a time, or you may get overlapping of the sounds being received, i.e. receiving the sound sent out by one of the other sensors.  I am also not interested in the distance if it is greater than 15cm (half a foot), as at this stage I only care about the things that the robot is about to hit, and as we can see from the maths, distances that are further away take longer to measure.

In my RefreshStatus() function (same format as the Obstacle Sensor, though still not done a Sensor base class...), you will see that I check the distance, and if it is less than 15cm, I set the recommended action to avoid obstruction (this is a new action added to the enum).

So need I need to update my AutonomousControl class to use the new sensors.  Here is the complete class:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Windows.UI.Xaml;

namespace Robot
{
    

    /// <summary>
    /// this class will evolve to handle multiple sensors and make decisions on what the robot should do,
    /// it may be split out into drive control and other functions
    /// </summary>
    public class AutomonousControl
    {
        public enum RobotState
        {
            Forward,
            AvoidObstructionLeft,
            AvoidObstructionRight
        }

        public AutomonousControl(MotorControl motorControlIn)
        {
            motorControl = motorControlIn;

            Initialise();
        }

        public void Initialise()
        {
            currentState = RobotState.Forward;

            frontUltraSonicSensor = new UltraSonicSensor(21, 20);
            leftUltraSonicSensor = new UltraSonicSensor(19,26);
            rightUltraSonicSensor = new UltraSonicSensor(13, 06);

            timer = new DispatcherTimer();
            timer.Interval = TimeSpan.FromMilliseconds(50);
            timer.Tick += RefreshDecision;
            timer.Start();
        }

        public void RefreshDecision(object sender, object e)
        {
            frontUltraSonicSensor.RefreshStatus();
            leftUltraSonicSensor.RefreshStatus();
            rightUltraSonicSensor.RefreshStatus();

            if ( (leftUltraSonicSensor.RecommendedAction == RobotAction.Forward) &&
                (frontUltraSonicSensor.RecommendedAction == RobotAction.Forward) &&
                (rightUltraSonicSensor.RecommendedAction == RobotAction.Forward) )
            {
                currentState = RobotState.Forward;
            }
            else if (  currentState == RobotState.Forward )
            {
                // there is a new obstruction
                if ( rightUltraSonicSensor.RecommendedAction == RobotAction.AvoidObstruction )
                {
                    // go left
                    currentState = RobotState.AvoidObstructionLeft;
                }
                else
                {
                    currentState = RobotState.AvoidObstructionRight;
                }
            }
            else
            {
                // we are currently trying to avoid an obstruction, so keep doing what we are doing!
                // (and hope for the best!)
            }

            if (currentState == RobotState.Forward)
            {
                RecommendedAction = RobotAction.Forward;
            }
            else if (currentState == RobotState.AvoidObstructionLeft)
            {
                RecommendedAction = RobotAction.Left;
            }
            else
            {
                RecommendedAction = RobotAction.Right;
            }   

            // do what is recommended!
            motorControl.Move(RecommendedAction);

        } 

        public RobotAction RecommendedAction
        {
            get;
            set;
        }

        /// <summary>
        /// a list of sensors derived from a sensor base object would be better
        /// </summary>
        private ObstacleAvoidanceSensor objectSensor;
        private UltraSonicSensor frontUltraSonicSensor;
        private UltraSonicSensor leftUltraSonicSensor;
        private UltraSonicSensor rightUltraSonicSensor;

        private RobotState currentState;
        private MotorControl motorControl;

        private DispatcherTimer timer;

    }
}

(Note: this is not done very well, but I am just playing with ideas at this stage.  I will give it a bit of a tidy up and re-factoring shortly...)

RefreshDecission() is where the clever stuff occurs.  In my first go at using multiple sensors, I went for the basic approach of if the left sensor is triggered go right, and if the right or forward sensors are triggered go left.  This seemed to make sense, but in using this logic I ended up with a problem where the robot would get stuck in a corner.  As the robot approached the corner the left sensor would trigger, so the robot would move a bit to the right, which would stop the left sensor being triggered, but cause the right sensor to be triggered, the robot would then move to the left, and you can see what would happen... the robot got stuck twitching left to right and back again...

To overcome this problem I have implemented a (very) basic state machine.  The Robot has three states:

  • Forward
  • AvoidObstructionLeft
  • AvoidObstructionRight

The plan being that once in one of the AvoidObstruction states, you can only go to the Forward state, so the robot will basically continue to rotate on the spot until it finds the way out, rather than toggling between rotating left and right.  You can see the code to achieve this in the RefreshDecision() function above.

To aid mounting the sensors on to the Robot, I stuck them into a breadboard, which I then attached to the front of the Robot using cable ties.

Here is a Video of the robot in action:

Next, I need to think about what to get it to do next!  I may have a look at line following or giving it some sort of task to perform? any ideas let me know!

Comments

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

Autonomous - Well Almost...

Autonomous

The first stage in making the Robot Automous is to add sensors, so that the robot knows what is going on around it, and is able to make decisions about what to do.  In this post I will add an IR (infra-red) sensor that can detect is something is in front of the robot.  For now, if it detects an obstruction it will stop.

To start with on my robot project I am just having a go with different things, I am starting with a Windows 10 IoT based platform that uses an Arduino to control the motors.  I expect that I will eventually move on to using ROS (Robot Operating System), but to start with I am just trying a few basic things out.  I don't know anything about how robots work, or the best ways to do things (and have not really done much research on such things), so for now I am just going to make it up as I go.  Things are not likely to be done in the best way, and feel free to correct or point out any improvements that I can make!

The IR sensor that I am using is a Keyes board, this came with a sensor pack that I got from Amazon.  It uses an IR led, and then uses a IR receiver to measure the reflected IR.  The board gives a digital output for object detected or not, and you can tune the distance that the object needs to be to the sensor using a potentiometer on the board (however the range only seems to be from 10cm to a about 2cm).  The Board sets the output pin high for no object and low for an object (fail-safe).  The board also has a handy LED that goes on/off to correspond with this value, which is great for tuning it and debug.

Because it is a digital signal, it is very easy to wire up to the Pi, using one of the Pi's GPIO pins (I have used GPIO4, pin 7, along with a GND pin and a 5v pin).  I have wrapped up talking to the sensor in a class, this really should have inherited from a base sensor class, but I was rushing!  Here is the code:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Windows.Devices.Gpio;

namespace Robot
{
    public class ObstacleAvoidanceSensor
    {
        

        public ObstacleAvoidanceSensor(int sensorPinIn)
        {
            RecommendedAction = RobotAction.Neutral;

            Initialise(sensorPinIn);
        }

        public void Initialise(int sensorPin)
        {
            var gpio = GpioController.GetDefault();

            // Show an error if there is no GPIO controller
            if (gpio == null)
            {
                pin = null;
                return;
            }

            pin = gpio.OpenPin(sensorPin);
            
            pin.SetDriveMode(GpioPinDriveMode.Input);
        }

        public void RefreshStatus()
        {
            pinValue = pin.Read();

            if ( pinValue == GpioPinValue.Low)
            {
                // object detected in front of this sensor so recomend stop
                RecommendedAction = RobotAction.Stop;
            }
            else
            {
                // no object detected so move forward
                RecommendedAction = RobotAction.Forward;
            }

        }

        public RobotAction RecommendedAction
        {
            get;
            set;
        }

        private GpioPin pin;
        private GpioPinValue pinValue;

    }
}

 In the constructor I allow the pin number to be passed in, this is so I can add multiple of these sensors in the future.  The Initialise function, gets the GPIO controller, and then opens the required pin, setting it to 'Input' mode. 

The RefreshStatus() function is a function that is to be called from the controlling task periodically to get the current value from the pin, and make a decision about what the robot should do.  I have created an Enum (Enumerator) of Robot Actions, these are things that the robot is able to do.  Here is my current list of actions:

public enum RobotAction
{
    Neutral,
    Forward,
    Reverse,
    Left,
    Right,
    Stop
}

The plan is that each sensor would set a recommended action based on its state, and another object would be responsible for looking at each of these recommended actions and picking the best one.  The object that will do this is a AutomousControl object, here is the current code for it:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Windows.UI.Xaml;

namespace Robot
{
    

    /// <summary>
    /// this class will evolve to handle multiple sensors and make decisions on what the robot should do,
    /// it may be split out into drive control and other functions
    /// </summary>
    public class AutomonousControl
    {
        public AutomonousControl(MotorControl motorControlIn)
        {
            motorControl = motorControlIn;

            Initialise();
        }

        public void Initialise()
        {
            objectSensor = new ObstacleAvoidanceSensor(4);  // use GPIO pin 4

            timer = new DispatcherTimer();
            timer.Interval = TimeSpan.FromMilliseconds(50);
            timer.Tick += RefreshDecision;
            timer.Start();
        }

        public void RefreshDecision(object sender, object e)
        {
            objectSensor.RefreshStatus();

            // here a decision will be taken based on the sensors
            RecommendedAction = objectSensor.RecommendedAction;

            // do what is recommended!
            motorControl.Move(RecommendedAction);

        } 

        public RobotAction RecommendedAction
        {
            get;
            set;
        }

        /// <summary>
        /// a list of sensors derived from a sensor base object would be better
        /// </summary>
        private ObstacleAvoidanceSensor objectSensor;

        private MotorControl motorControl;

        private DispatcherTimer timer;

    }
}

This class is passed in the MotorControl object (see previous post, though I have changed it), and starts up a DispatchTimer as part of its initialisation.  I have set the timer to go off every 50 msec.  This was the first value I came up with, but seems to be okay and stable.

The Timer tick function (RefreshDecision), calls the RefreshStatus() function on the sensor, and then just sets the recommended action to be the one that, that sensor is recommending (as I only have one sensor at the moment).  It then passes the recommended action to the motor controller move function.  This is a new function that takes in a RobotAction value.  Here is the complete updated MotorController class, as I have also had to make some other modifications:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Robot
{
    public enum RobotAction
    {
        Neutral,
        Forward,
        Reverse,
        Left,
        Right,
        Stop
    }

    public class MotorControl
    {
        public delegate void MovementFunction();

        public MotorControl(SerialDriver serialDriverIn)
        {
            serialDriver = serialDriverIn;
            SetLowSpeed(); // default to low speed
            
            RobotActions = new Dictionary<RobotAction, Delegate>();
            RobotActions.Add(RobotAction.Forward, new MovementFunction(Forward));
            RobotActions.Add(RobotAction.Left, new MovementFunction(Left));
            RobotActions.Add(RobotAction.Right, new MovementFunction(Right));
            RobotActions.Add(RobotAction.Reverse, new MovementFunction(Backwards));
            RobotActions.Add(RobotAction.Stop, new MovementFunction(Stop));
            RobotActions.Add(RobotAction.Neutral, new MovementFunction(Stop));
        }

        public void Move(RobotAction action)
        {
            RobotActions[action].DynamicInvoke();
        }

        // NOTE: I WIRED MY ROBOT BACKWARDS, AND FIXING IN SOFTWARE IS EASIER THAN HARDWARE :) 
        public void Forward()
        {
            SetSpeed(demandSpeed);
            serialDriver.WriteAsync("s");
            turning = false;
        }

        public void Backwards()
        {
            SetSpeed(demandSpeed);
            serialDriver.WriteAsync("w");
            turning = false;
        }

        public void Right()
        {
            SetSpeed(9); //set max speed to turn
            serialDriver.WriteAsync("a");
            turning = true;
        }

        public void Left()
        {
            SetSpeed(9); //set max speed to turn
            serialDriver.WriteAsync("d");
            turning = true;
        }

        public void Stop()
        {
            serialDriver.WriteAsync("x");
            turning = false;

        }

        public void SetHighSpeed()
        {
            if ((demandSpeed != HighSpeedValue) && (turning == false))
            {
                SetSpeed(HighSpeedValue);
            }

            demandSpeed = HighSpeedValue;
        }

        public void SetLowSpeed()
        {
            if ( ( demandSpeed != LowSpeedValue) && ( turning == false ) )
            {
                SetSpeed(LowSpeedValue);
            }

            demandSpeed = LowSpeedValue;
        }

        private void SetSpeed(byte speed)
        {
            serialDriver.WriteAsync("" + speed);
        }


        private const byte LowSpeedValue = 5;
        private const byte HighSpeedValue = 9;
        private bool turning;
        private byte demandSpeed;
        private SerialDriver serialDriver;
        public Dictionary<RobotAction, Delegate> RobotActions;
    }
}

I will start with the Move function; I created a dictionary of RobotActions to function pointers (Delegates), which is populated in the Initialisation of this class.  This allows me to invoke the required method based on the passed RobotAction value in the move function (without the need for a messy switch statement or else if, else if, else it...).

You may have noticed in my previous posts that my robot was going backwards!, I put this down to wiring my motors backwards, and as I am a software engineer rather than a hardware engineer, I fixed the problem in code, by swapping the serial commands around...

I also found that I needed to lower the speed of the forward and reverse of the robot, so I have added a low and high speed setting.  This has been implemented so that it only effects the speed of forward and reverse as I needed full speed to turn the robot (due to the weight of the Pi Battery).  In order to do this, I had to also update the Arduino code to take in char values 1 - 9 to set the speed, where 1 is very slow, and 9 is max (well just below due to rounding).  Here is the updated code:

/*
 # Editor : Phoebe
 # Date   : 2012.11.6
 # Ver    : 0.1
 # Product: Cherokey 4WD Mobile Platform
 # SKU    : RBO0102
   
 # Description:     
 # Drive 2 motors with this Cherokey 4WD Mobile Platform  
 # Connect the D4,D5,D6,D7,GND to UNO digital 4,5,6,7,GND
 
*/
  
int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 4;     //M1 Direction Control
int M2 = 7;     //M1 Direction Control
char speed = 255;

 
void stop(void)                    //Stop
{
  digitalWrite(E1,0); 
  digitalWrite(M1,LOW);    
  digitalWrite(E2,0);   
  digitalWrite(M2,LOW);    
}   
void advance(char a,char b)          //Move forward
{
  analogWrite (E1,a);      //PWM Speed Control
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}  
void back_off (char a,char b)          //Move backward
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);   
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
void turn_L (char a,char b)             //Turn Left
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}
void turn_R (char a,char b)             //Turn Right
{
  analogWrite (E1,a);
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
 
void setup(void) 
{ 
  int i;
  for(i=4;i<=7;i++)
    pinMode(i, OUTPUT);  
  Serial.begin(19200);      //Set Baud Rate
  Serial.println("Run keyboard control");
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW); 
 
} 
  
void loop(void) 
{
  
  
  if(Serial.available())
  {
    char val = Serial.read();
    if(val != -1)
    {
      switch(val)
      {
      case 'w'://Move Forward
        advance (speed,speed);   //move forward in max speed
        break;
      case 's'://Move Backward
        back_off (speed,speed);   //move back in max speed
        break;
      case 'a'://Turn Left
        turn_L (speed,speed);        
        break;       
      case 'd'://Turn Right
        turn_R (speed,speed);
        break;
      case 'z':
        Serial.println("Hello");
        break;
      case 'x':
        stop();
        break;
      default: // check if a new speed has been passed.
        if ( ( val >= '1' ) && ( val <= '9' ) )
        {
          char speedValue = ( val - '1' ) + 1 ;
          speed = 28 * speedValue; //Note: 255 / 9 is 28
          Serial.println(speed);
        }
      }
    }
    else stop();  
  } 
}

Note: that this is code from DFRobot, which I have modified a bit.

Next you just need to create the AutomousControl object from the Main Page class (where I create the RobotWebServer object), and then that should be it!

Once this is all flashed on to the Arduino and the Pi, you should get something like this:

I have placed an order for some Ultrasonic distance sensors, these should enable objects to be detected from a greater distance, which means I can control the speed of the robot based on distance.  I can then also look at making the robot go around an obstacle, rather than just stopping.

Comments

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

This Time an Actually Moving Robot!

This Time an Actually Moving Robot!

Okay, last time I had the robot 'moving' but not going anywhere (due to being tethered to my laptop by a USB cable), this time I have got the robot moving on the floor! wirelessly!

So to do this I have added a Raspberry Pi to the bot, the intention is that this will eventually be the brains of the robot, and be responsible for monitoring various sensors and sending the drive commands to the Arduino, which does the PWM etc. to talk to the Motor Driver.  To start with, the plan is to just add wireless control of the robot via the Pi.  The quickest (but not necessarily the best) way to achieve this is to make use of the code that I did for my Christmas Light Toy Project, this allowed control of a set of Christmas lights via a HTTP Web server.  The plan is to take this code and modify it to allow control of the robot, via a web browser.  I say this is not the best way, as this method will have a lot of latency associated with it, this means that when you press stop on the browser, it could take up to a second (longer if the wifi decides to be a pain), to get to the Pi, and then on to the Arduino and then the Motor Driver and then the Motors...  My plan is really just to use this as a temporary proof of concept thing, the plan is for the robot to be autonomously controlled, so it will hopefully not need me to tell it to do anything instantly!  And there will be a lot of benefit in having a web server on it, as I will be able to set control/operation modes and also view debug and diagnostic information.  There is also potential to fit a web cam on it and server the images to a browser via the Web Server.

The Pi is fitted to the top platform of the robot, I had to use fittings I had lying around, as I used up all the stuff that came with the robot kit to fit the Arduino.  To power the Pi, I am using a Anker USB charger, this is very heavy for a robot, but again it is a thing I already have rather than purpose built.  This is simply held to the bottom of the top platform by cable ties.

Wiring is straight forward, you need to connect the Pi's UART TX and RX pins (8 and 10 on the Pi 2) to the RX and TX on the Arduino (digital pins 0 and 1).  You must also connect a ground (GND) pin on the Pi to one on the Arduino, I went for Pi pin 6 to Arduino pin GND (on the same block as the digital ones, though it shouldn't matter which you use).

Right, on to the code!  So If we start at the lowest level and work up, the first part was a SerialDriver class, this is used to wrap up the serial comms to the Arduino.  Here is the code:

using System;
using System.Collections.ObjectModel;
using Windows.UI.Xaml;
using Windows.UI.Xaml.Controls;
using Windows.Devices.Enumeration;
using Windows.Devices.SerialCommunication;
using Windows.Storage.Streams;
using System.Threading;
using System.Threading.Tasks;

namespace Robot
{
    public class SerialDriver
    {
        private SerialDevice serialPort = null;
        DataWriter dataWriteObject = null;
        DataReader dataReaderObject = null;

        private ObservableCollection<DeviceInformation> listOfDevices;
        private CancellationTokenSource ReadCancellationTokenSource;

        public SerialDriver()
        {
            initialise();         
        }

        public async void initialise()
        {
            string aqs = SerialDevice.GetDeviceSelector();
            var dis = await DeviceInformation.FindAllAsync(aqs);

            DeviceInformation entry = (DeviceInformation)dis[0];

            try
            {
                serialPort =  await SerialDevice.FromIdAsync(entry.Id);

                // Configure serial settings
                serialPort.WriteTimeout = TimeSpan.FromMilliseconds(1000);
                serialPort.ReadTimeout = TimeSpan.FromMilliseconds(1000);
                serialPort.BaudRate = 19200;
                serialPort.Parity = SerialParity.None;
                serialPort.StopBits = SerialStopBitCount.One;
                serialPort.DataBits = 8;
                serialPort.Handshake = SerialHandshake.None;

                // Create cancellation token object to close I/O operations when closing the device
                ReadCancellationTokenSource = new CancellationTokenSource();

                // Create the DataWriter object and attach to OutputStream
                dataWriteObject = new DataWriter(serialPort.OutputStream);
            }
            catch (Exception ex)
            {
                // Really must do something here!
            }
        }

        public async Task WriteAsync(string toWrite)
        {
            Task<UInt32> storeAsyncTask;

            if (toWrite.Length != 0)
            {
                // Load the text from the sendText input text box to the dataWriter object
                dataWriteObject.WriteString(toWrite + Environment.NewLine);

                // Launch an async task to complete the write operation
                storeAsyncTask = dataWriteObject.StoreAsync().AsTask();
            }           
        }
    }
}

I created this class from one of the Windows 10 IoT samples.  There are two parts to it.  The first is the initialisation in the initialise() function.  This first searches for available serial ports, and then uses the first one (for now I am making the assumption that there is only one port, this may change when I start adding sensors).  The comm port is then selected and all the required serial comm parameters are set so that they match what the Arduino expects.  The final part is to create a DataWriter (.Net class) object from the serial port's output stream.

Once this is done, the second part is a WriteAsync() function, this function can be called from elsewhere in the code, to send a message to the serial port (and on to the Arduino).  The function uses the WriteString function of the DataWriter object that we created to send the passed string.  The actual sending of the data is handled in the background by a call to the StoreAsync() function.

So the SerialDriver class is used by the next level up, which is the MotorControl class.  This is a simple class which wraps up the 'w','s','a','d','x', used to control the motor, into C# functions, such as Forward, Backwards, etc.  This may seem like extra work, as why not just call the WriteAsync("w"); function, in your code where ever you want the robot to go forward.  But by adding this layer, you can use the same higher level code to easily control a different type of robot, without needed to change (or may be even compile it, depending on how you arrange your project).  It is always good practice to include these sorts of layers, and is part of good object orientated design.  Besides, it is not very readable to see WriteAsync("w"); all over your code, whereas Forward() allows the reader to instantly understand what the function will do.

So here is the MotorControl class (I don't think it needs any further explanation):

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Robot
{
    public class MotorControl
    {
        public MotorControl(SerialDriver serialDriverIn)
        {
            serialDriver = serialDriverIn;
        }

        public void Forward()
        {
            serialDriver.WriteAsync("w");
        }

        public void Backwards()
        {
            serialDriver.WriteAsync("s");
        }

        public void Right()
        {
            serialDriver.WriteAsync("d");
        }

        public void Left()
        {
            serialDriver.WriteAsync("a");
        }

        public void Stop()
        {
            serialDriver.WriteAsync("x");

        }

        private SerialDriver serialDriver;
    }
}

So now we can control the Robot with easy to use functions from the rest of the project.  The next part is to add the HTTPServer.  I won't go to much in to this, if you want to understand how it works you can view my post on Windows 10 IoT HTTP Web Server.

The only parts of the code I changed from what I used in the Christmas Light Toy, was to remove all the Light Initialisation stuff in the constructor and corresponding member variables, and replace them with an instance of MotorControl, which I pass in to the WebServer's constructor.  I also modified the ParseResponse() function to use the MotorControl object:

private string PrepareResponse(string request)
{
    string response = "ERROR";

    response = defaultPage;

    if (request == "")
    {
        // this will be called when the Root (http://robot/) is requested
    }
    // check to see if the request is a known colour
    else if (request == "forward")
    {
        motorController.Forward();
    }
    else if (request == "backward")
    {
        motorController.Backwards();
    }
    else if (request == "left")
    {
        motorController.Left();
    }
    else if (request == "right")
    {
        motorController.Right();
    }
    else if (request == "stop")
    {
        motorController.Stop();
    }
    
    return response;
}

(Note: that robot is the device name of the raspberry pi, you can use its IP address if you prefer)

(Also: Note: I know that code is rubbish and should be using a factory instead of a switch style if statement...)

(Also, Also, Note: I really should have created the original HTTPServer class so that the Christmas Light Toy and Robot projects could have just inherited from it...)

Okay, so I also needed to update my mainpage to have robot controls on rather than Christmas Light controls, I did a quick tweak and ended up with this:

<!DOCTYPE html>

<html lang="en" xmlns="http://www.w3.org/1999/xhtml">
<head>
    <meta charset="utf-8" />
    <title>Robot Controller</title>
    <style type="text/css">
        div {
            position: relative;
            height: 100px;
        }

        .button {
            position: absolute;
            height: 100%;
            width: 100%;
            left: 0;
            top: 0;
        }
    </style>
</head>
<body>
    <table border="0" cellpadding="0" cellspacing="0" style="width: 100%;">
        <tbody>
            <tr>
                <td><div id="button" class="nav">&nbsp;<a href="/"><span class="button" style="background-color: #ffffff"></span></a></div></td>
                <td><div id="button" class="nav">&nbsp;<a href="/forward"><span class="button" style="background-color: #0000ff"></span></a></div></td>
                <td><div id="button" class="nav">&nbsp;<a href="/"><span class="button" style="background-color: #ffffff"></span></a></div></td>
            </tr>
            <tr>
                <td><div id="button" class="nav">&nbsp;<a href="/left"><span class="button" style="background-color: #ff0000"></span></a></div></td>
                <td><div id="button" class="nav">&nbsp;<a href="/stop"><span class="button" style="background-color: #000000"></span></a></div></td>
                <td><div id="button" class="nav">&nbsp;<a href="/right"><span class="button" style="background-color: #00ff00"></span></a></div></td>
            </tr>
            <tr>
                <td><div id="button" class="nav">&nbsp;<a href="/"><span class="button" style="background-color: #ffffff"></span></a></div></td>
                <td><div id="button" class="nav">&nbsp;<a href="/backward"><span class="button" style="background-color: #0000ff"></span></a></div></td>
                <td><div id="button" class="nav">&nbsp;<a href="/"><span class="button" style="background-color: #ffffff"></span></a></div></td>
            </tr>
            
        </tbody>
    </table>

</body>
</html>

Which looks a bit like this on the screen:

The layout is based on a cross, with the top and bottom blue button going forward and backwards, red for left and green for right, and then black in the middle for stop. 

And that is it!, well I thought it was, but then it turned out that I also needed to add the serial comms capability to the Package.appxmanifest file.  This can be found in the Solution Explorer in the top level of your main project (the one that is bold, you may only have one). You need to right click and then 'View Code' as unlike adding the internetClientServer capability, the serial one is not on the user interface.  Ensure that you capabilities element contains the following:

 <Capabilities>
    <Capability Name="internetClient" />
    <Capability Name="internetClientServer" />
    <DeviceCapability Name="serialcommunication">
      <Device Id="any">
        <Function Type="name:serialPort" />
      </Device>
    </DeviceCapability>
 </Capabilities>

And now that is it!

Here is the robot being controlled (no wires!, apart from all the ones on the actual robot...)

Next, it is time to think about sensors! 

Comments

Awesome. Can't wait to get to this point.

Respond

Join the discussion

Log in or create an account, and start talking!

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

Moving Robot!

Moving Robot!

Well, Okay, I didn't manage to get it to actually move very far, but I can make the wheels go around!

My original plan was to attempt to control the robot direct with a Raspberry Pi running Windows 10 IoT, this would involve using digital GPIO to set direction (easy), and PWM to control the motor speed (problematic).  The issue would be that the Winiot does not support hardware PWM and using software PWM on winiot would probably cause problems, as it would not be able to achieve the timing requirements.  I did a bit of research, and I did find so things that looked promising, but due to the fact that I had an Arduino lying around that I have never used, and also that the sample code provided by DFRobot to run on an Arduino looked very straight forward, I decided to go with that.

So, here are the steps I went through to get it going.

First, I had to wire up the motors, and add the battery holder:

I connected the following pins on my Arduino to the Cherokey's motor driver:

Arduino (digital input)Cherokey (motor driver input)
4D4
5D5
6D6
7D7
GNDGND

Note: I did not connect the Rx and Tx ports on the motor controller to the ones on the Motor Driver.  I was not sure what these would be for, and it seemed to work okay without them connected.  

Next I needed to install the Arduino IDE, which you can download from here: https://www.arduino.cc/en/Main/Software Once this was installed, and running, I next plugged in a USB cable connecting my laptop to the Arduino (Note: I unplugged the drum power connector that would normally go from the Cherokey's batteries to the Arduino as the USB would power the Arduino.  You must also ensure that the Arduino GND pin is connected to the Cherokey's GND pin, as mentioned in my table above).

The laptop detected the Arduino and assigned it to COM5.  If it is not obvious from the Arduino IDE, which COM port has been assigned, you can find out (in Windows), by looking in Device Manager (search for 'Device Manager' in the start menu search thing, once device manager has started you should be presented with a tree view of all the computers devices, if you expand the Ports (COM & LPT), you should see something like Arduino Uno (COM5).

Next copy and paste the following code in to the Arduino IDE (you may then wish to save it as a Project).

/*
 # Editor : Phoebe
 # Date   : 2012.11.6
 # Ver    : 0.1
 # Product: Cherokey 4WD Mobile Platform
 # SKU    : RBO0102
   
 # Description:     
 # Drive 2 motors with this Cherokey 4WD Mobile Platform  
 # Connect the D4,D5,D6,D7,GND to UNO digital 4,5,6,7,GND
 
*/
  
int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 4;     //M1 Direction Control
int M2 = 7;     //M1 Direction Control
 
void stop(void)                    //Stop
{
  digitalWrite(E1,0); 
  digitalWrite(M1,LOW);    
  digitalWrite(E2,0);   
  digitalWrite(M2,LOW);    
}   
void advance(char a,char b)          //Move forward
{
  analogWrite (E1,a);      //PWM Speed Control
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}  
void back_off (char a,char b)          //Move backward
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);   
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
void turn_L (char a,char b)             //Turn Left
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}
void turn_R (char a,char b)             //Turn Right
{
  analogWrite (E1,a);
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
 
void setup(void) 
{ 
  int i;
  for(i=4;i<=7;i++)
    pinMode(i, OUTPUT);  
  Serial.begin(19200);      //Set Baud Rate
  Serial.println("Run keyboard control");
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW); 
 
} 
  
void loop(void) 
{
  if(Serial.available())
  {
    char val = Serial.read();
    if(val != -1)
    {
      switch(val)
      {
      case 'w'://Move Forward
        advance (255,255);   //move forward in max speed
        break;
      case 's'://Move Backward
        back_off (255,255);   //move back in max speed
        break;
      case 'a'://Turn Left
        turn_L (100,100);        
        break;       
      case 'd'://Turn Right
        turn_R (100,100);
        break;
      case 'z':
        Serial.println("Hello");
        break;
      case 'x':
        stop();
        break;
      }
    }
    else stop();  
  } 
}

The code is from the official DFRobot wiki page for the Cherokey, I have to admit, that as I have never used an Arduino before, I looked at the above code, and instantly thought, that is not going to compile!! (no #includes!), but it did, obviously by some Arduino IDE magic!?!?  Next I set the board by selecting Tools -> Board -> Arduino Uno in the menu, and then selected the COM port, by Selecting Tools -> Port -> COM5.

Now, the code can be flashed on to the Arduino by selecting Sketch->Upload from the menu.  Hopefully you get a 'Done Uploading' in the status bar (which is between the editor area and the output area).  Next you want to open the serial terminal by selecting Tools -> Serial Monitor, in this you will want to select a baud rate of 19,200, this is done from a drop list in the bottom right of the Serial Monitor Window.  This should allow communication with the device, which can be tested by typing 'x' then hitting return in the Serial Monitor.  You should see a reply of 'Hello' in the Serial Monitor.

This is a good point to add the batteries to the Cherokey and switch it on!  First you should see 7 white LEDs on the Cherokey light up, these are just for decoration, I don't think you can control them.  If you have a short USB cable like me, then you should keep the wheels off the ground, as using this method to control the Robot is slow as it requires two keyboard presses per command.

You should now be able to control the robot using: w for forward, s for backwards, a, for left, d for right, and x to stop.

Here is a video of mine working:

As I mentioned, I have managed to get the wheels turning, but it is not really going anywhere with the USB cable tethering it to my laptop!  So, the next job, is to rewrite the above, so that I can talk to it via a Raspbery Pi, rather than my laptop.  I can then look at various ways to control it wireless and then eventually work towards making it Autonomous!

Comments

jwsv61099 commented on Moving Robot!:

Hey Ollie,
So I am working on a project similar to what you have done. I wanted to thank you for your very detailed information, as it is very helpful. I do have a question though; so when I run the code provided, only 6 of the 7 LED's light up, the 7th one being the largest one in the center. Along with that, the wheels do not rotate.... hopefully I will figure it out soon because it is frustrating... I have checked all of the wiring, and it all seems to be correct... I am also planning on adding a raspberry pi, some ultrasonic sensors, speakers and some cameras so this machine has some degree of autonomy to it.
Thanks!

jwsv61099 replied:

Okay, update:
When tampering with the Cherokee, I put a screwdriver on the positive side of where the wires from the battery pack go into and all of the lights it up, and the fourth wheel (M4) was the only one moving with the code provided. Even pulling and moving the positive wire caused the LEDs to light up and the wheel to move. Why though I'm not sure....

jwsv61099 replied:

I figured it out; it was as stupid mistake.
Thank you so much for your awesome tutorials.

Respond

Join the discussion

Log in or create an account, and start talking!

bmultak commented on Moving Robot!:

How did you connect the Arduino to the platform. Did the kit come with the male/female wires connecting the Arduino to the 4WD Cherokey? Or did you have to buy it separately?

Ollie replied:

Hi, I cant be totally sure, but I don't think it came with the wires, i had a lot already. I know that it definitely didn't even come with the power wires to go from the motor controller tomthe motors.

Respond

Join the discussion

Log in or create an account, and start talking!

GianInter25 commented on Moving Robot!:

hello I recently received one Cherokee 4wd.
I noticed a little problem. The robot can not do rotations as it should. It's lacking power. Use 5 x 1.5V AA batteries. Do you think that not enough? I would also like to know what is in the internal axis of the motors.
Sorry for my bad English but i'm italian. Thank you.

Ollie replied:

Hi, yes, I had the same problem, I could only get it to rotate when driving the motors at top speed, and even then as you can see in my videos it struggles a bit. Anything less that the full '255' output and it would stall. In the sample code I think they suggest that you can get away with values of '100', this did not work for me.

I have only tried in on a hard surface, I suspect it may struggle on carpet or outside.

I am using 5 AA batteries, which are 2000mAh rechargeable ones. I don't know much about the motor driver they are using, but there may be a way to tune it to put more power to the motors. I didn't see that in any of DFRobots documentation though.

Not sure about how the motor is configured, other than it has a 1:120 gear ratio.

Respond

Join the discussion

Log in or create an account, and start talking!

griz11 commented on Moving Robot!:

Have you thought about using ROS on the PI? A bit of a learning curve especially if you have to learn Linux too but once you get used to it you will love it.

Ollie replied:

Yeah, the plan is to eventually use ROS on the Pi, but just wanted to have a bit of a play with some basic sensors, just to see what I can do.

Respond

Join the discussion

Log in or create an account, and start talking!

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

The Robot

The Robot

Okay finally got my robot chassis and motors today, despite royal fail's efforts... It is a DFRobot Cherokey 4WD robot kit.  This includes the motor driver so that you can control the motors directly from a device such as an Arduino using digital IO.  Note that PWM is required to set the speed of the motors.  I am intending to have a go at controlling it with a Raspberry Pi.  I may need to do this via an Arduino.  I am also intending to use Windows 10 IoT on the Pi. 

Here are the contents:

In the kit you basically get:

  • 4 Wheels
  • 4 Motors with gears and axle to mount the wheels
  • The chassis (which is metal, aluminium I think)
  • The motor driver board, which has mount points for an Arduino (and a few other boards)
  • A battery holder (5 AAs)
  • A test/development board, which includes mounting points and hole where components can be mounted
  • Fixtures and fittings to hold it together.

Here is a close up of the motor driver board  The space in the middle is a location where you can mount your microcontroller board.  I am going to try to fit a Pi here, but I am have to fit it to the top board:

The Wheels are slightly bigger than I thought, and have rubber tires, which seem quite good:

There are no instructions included in the kit, but there is info on DFRobot's website.  Putting it together is straight forward:

First fit the motors to the side pieces, using the long screws and nuts:

Next the wheels can be fitted to the axles.  These simply push on.

Next fit the two sides together using the 4 metal structs and screws:

At this point you want to add the wires for the motors, but I don't have any suitable wire at the moment, so I will need to come back to this stage!

Once you have fitted your motor wires, you can add then add the motor controller base board.  This is attached using the long plastic spacer screws.  There are 6 of these, though I have only added 4 as I am going to need to remove the board to solder on the motor wires:

Next you can fit the battery holder to the underside of the motor controller board, and the top board on to the spacer screws.  But I will do this later.

So that is the kit.  There are a few things I need to get, some wire and another Pi (as my other one is currently controlling my Christmas Lights!).

Next I will look at getting it moving!

Comments

griz11 commented on The Robot:

Looks good. Have you fired it up yet?

Ollie replied:

Just having a go with it now!, if it works I will do another post!

Respond

Join the discussion

Log in or create an account, and start talking!

Post a Comment!

Join the discussion

Log in or create an account, and start talking!

Ollie made a post to 4WD Robot:

The Plan

The Plan

Having been inspired by griz11's'Elroy' Jetson Autonomous Rover project, I have decide to have a go at robotics!  So far I have spent a bit of time looking at various kits and stuff and what other people have been up to, and chatted to griz11 a bit for advice.

To start with I am just going to have a go at the basics, and see how that goes, with a plan to do something a bit more advanced, using a bigger robot base and using ROS (Robot Operating System - not an OS, but a collection of Libraries and software tools used to the development of Robots).

So this initial project will just focus on the basics, getting a very simple 4 wheeled robot moving autonomously, trying out a variety of sensors on a small scale to see what works, and how to do it.

This is the robot I have selected:

It is a DFRobot Cherokey 4WD Mobile Platform.  It has a motor for each wheel, and includes a built in motor controller, which makes it easier to find a compatible one!  The wheel are fixed in position, so differential steering is used, this is a similar method to how a tank turns, i.e. to turn left, the left wheels are rotated in reverse and the right wheel forward.  This enables the robot to spin on the spot, which also makes using sensors to control movement a bit easier to manage, as you can turn away from an obstruction in a small space.  This model does not include any wheel encoders but does have the brackets to fit them.  I expect that I will need to do this, because the motors are unlikely to work in a uniform way.  I.e. sending 4.5V to two motors, is unlikely to result in the exact same speed out of both, and it is also likely to change as the batteries drain etc.  A wheel encoder is a way of measuring the wheel speed this means they can be compared and adjustments made to the voltage accordingly.  I am sure that there will be more on this in a future post...  

I am intending to have a go at using a Raspberry Pi to control the motors via the motor driver electronics.  This may prove to be problematic, this is because the motor driver requires a PWM (pulse width modulation - in this case use as a way to simulate a analogue output using a digital signal) input to control the speed.  I believe the Pi can do PWM and has two channels available (I think one may be shared with the audio, but I don't think I will be needing that), but not sure if the quality of the waveform generated will be good enough to smoothly drive the motors.  I am going to give it a go.  I will probably also try software PWM too, but I suspect that it will be a no go.  If the Pi's PWM is no good, then the plan will be to use an Arduino, that will handle the PWM etc. I can then use a Pi to send commands to the Arduino, either using serial comms (slower, but easier) or I2C (faster but harder). 

Power will be simply provide by rechargeable AA batteries in a battery pack.  I may consider use of a high capacity LiPo battery if i find the AA's run down too quick.  The Pi will probably be powered from a portable USB charger battery pack (such as an Anker one).   I also intend to include a LCD screen on the Pi, which will be able to provide diagnostic/debug information during testing.

Here is schematic of the motor driver board:

The initial plan is just to get it moving and be able to control it, probably using the HTTP server in this post.  Yes, I do intend to use Windows 10 IoT again!, though it is very likely when I take things bigger I will want (need) to use ROS, which I believe Ubuntu (Linux) is recommended/required.  The HTTP server will be a very poor way to control the robot, due to the significant lag between request a direction from the client browser to the signal actually getting to the motor (especially if there is also a comms link to an Arduino in the loop).  Once this part has been proven I will then work toward getting it to work autonomously, this will be involve the use of a variety of sensors, to allow the robot to either follow things, or avoid things (hopefully!).

Comments

Post a Comment!

Join the discussion

Log in or create an account, and start talking!