Programming the robot- part2 (giving life to little Franky)

In my last  post I have discussed several functions that I have used. From this post I would like to discuss the rest of the functions.

Functions associated with LCD Display

Write_LCD()

Write_LCD() handles displaying the current distance measurement the ultra sound sensor is taking. Since the sensor measures distance every 50ms that resolution is too fast for humans to be able grasp any information. Therefore the actual display doesn’t show the distance in real time. It has an interval about 200ms.

First challenge I faced was that variable x is an integer so I had to convert it to a string before I can send it to LCD display. By using following code I was able to convert x into a string (i.e.an array of characters).

Converting an integer into an array

Another problem I faced was since x varies from single digit to up to 3 digits the display showed meaningless characters when there are blanks. For example let’s at the beginning x was a 3 digit number. When it changes to a 3 digit number the leftmost digit will turn in to a meaningless character. My solution was to use if statements to split the case into 3 scenarios. So if x is between 0 and 9, 2 zeros will be displayed in front of x, if x is between 10 and 99, a zero will be displayed in front of x, if x is larger than 100 no zeros will be displayed in front of x. you will be able to understand my method clearly.

Writing into LCD Display

Functions associated with servo motor

Control_servo()

Servo motor is used when robot has stopped due to detection of an obstacle and needs to find an alternative path to go. I used a polar coordination system to identify the angle and distance to an obstacle. The following diagram will help you to understand my method.

In this picture r denotes the displacement to the obstacle and theta  denotes the angle from left side horizontal axis. When the sensor facing forward direction value of  theta  must be 90 degrees. But I found that the minimum value for theta was about 20 degrees and maximum was about 150 degrees. Therefore when initializing the servo it was fed with 70 degrees not 90 degrees. Servo library takes the angle difference from it’s current position to move to a new position.

When the robot got a x value which is less than 20cm it stops and main function calls control_servo() function . Control servo() function first moves sensor from 70 degrees to 140 degrees. While doing this it records the distance when angle is 90,110 and 130 degrees. Then again sensor moves from 130 degrees to 0 degrees. Sensor records the distance when angle is 50, 30, 15 and 0 degrees. One important thing to remember is to have a delay when changing angle. Since the actual servo takes some time to move to a new position from current position a delay is required. I have used a delay of 10ms for this. By accurately measuring the angular speed of the servo this time delay can be fine-tuned. Data array is used to record relevant angle and displacement. 

Function to control Servo motor

Now I think it’s ok to explain functionality of turn_robot() function. The purpose of this function is to turn the robot in to a suitable direction based on the values in data array. When turn robot is called, first it calls control_servo() function in order to collect distance and angle measurements. Then it calls sort() function to arrange obtained data from smallest distance value to highest distance value. After this data[6] element will have the highest distance measurement and corresponding angle.70 degrees is subtracted from this angle. If this value is a negative value it means this angle (i.e. the obstacle that is situated furthest from robot) is positioned from left side of the robot. If this value is positive it means this angle (i.e. the obstacle that is situated furthest from robot) is positioned from right side of the robot. Then this angle is used to turn the robot to that direction.

equation 2

According to equation 2  there is a time period which is proportional to a given angle theta. Assuming omega is a constant which is the angular velocity of the robot turning then t can be used to turn the robot. By calculating a time period which corresponds to theta and using that time period as a delay period I was able to achieve this. We only need the magnitude of theta since we already know the direction of the turn. The angular velocity of the vehicle had to be measured experimentally and also this constant value is valid only when the surface that robot moves is uniform. So this method is not very accurate method at the moment but I am planning to implement a control system to keep the speed of the motors at a constant rate. By that we omega value will stay the same. In here I have multiplied turn_time variable by 1000 in order to convert it in to milliseconds.

Function to turn robot 

  Now I have covered all the functions I have constructed to build my program. The main program or loop() doesn’t do much except coordinating the all these functionalities and making robot move forward. Inside loop() it checks  x value .If x is larger than 20cm(or any arbitrary value) robot will move forward and if x is less than that value it will stop robot and call turn_robot() function. After that turn_robot() function will turn the robot in a suitable direction and starts main program from beginning.

Main function loop()

This is my complete program for my robot. Although this program works it still need lots of debugging. There are some unexpected behaviours that need to be fixed.It might take some time and I hope to write my progress on my blog. But I’m glad finally I was able to make a working prototype of my robot. It may sound funny but building this robot was like raising a child for me. You need to be both knowledgeable and passionate at what you are doing. Sometimes you get frustrated when things are not going well or not giving the desired output but you won’t give up until you are satisfied. You will realised even the simplest things in life like going on a straight line is not simple as it seems for somebody as simple as a robot. It takes lot of time and patience to achieve what you want but at the end of the day you can see that little creature is actually wondering in your room and this will make you feel you achieved what you deserve.

I think if you read these articles it might be helpful to you in your own projects. I have posted a link to the source code at the end of this article. Please feel free to download it and use it if you need.

follow this link to my source code:

Programming the robot -1(aka giving life to little Franky )

I was hoping to write about this earlier but due to many reasons I missed it and finally I got a chance to sit down continue on my post. In previous article I discussed how to connect a LCD display to Arduino board through a shift register. (74HC595N). I decided to have an LCD display in my robot for several reasons.

•    It’s very useful to know the readings that robot is taking during testing.
•    I can’t access serial monitor when my robot is not connected to PC. I  have to use a USB cable and it limits the robot’s movements.
•   It’s cool to have an extra gadget on my robot. It makes it more interactive.

So with the implementation of LCD display the robot consists of 4 main sections.

•    Motors and the IC that controls motors.
•    LCD display and shift register.
•    Distance sensor
•    Servo motor

In order to control all these sections I had to write a program that controls each of this section. I have talked about the motor controls in details in a previous post and the basics of implementing the LCD display and Distance sensor were discussed in last post. Therefore I would like to talk about the rest of my program in details in upcoming posts.

In this version the robot was expected to go along a path until it detects an obstacle from a certain distance. The robot then stops and starts to scan the surrounding area using the detector attached to a servo motor. Servo motor turns the detector to left and right while the detector records 7 measurements (distance to a particular obstacle and the direction i.e. angle to it with respect to the initial position of the detector. ) then it’s chooses the most suitable direction to go (direction which the distance to an obstacle is maximum.).It may seems like an easy thing to do but implementing it was indeed a difficult task.

Since this is going to be a bit lengthy than a usual post I would like to split this into small sections where I explain the functions that I implemented in order to control my robot. Here is a list of functions that I have used in program.

Functions associated with movements of robot

•   Go_right ()
•    Go_left ()
•    Go_backward()
•    Go_forward()
•     robot_stop()
•     turn_robot()

Functions associated with distance sensor

•  measure distance()
•  sort()

Functions associated with LCD

•  write_LCD()

d     Functions associated with servo motor

•        control_servo()

            Then there are setup () and main () functions that are being used to initialise and run program. 

      

      Including required libraries and declaring variables 

             

Header files used in program

 In this version of robot I used timer 2 in AVR micro controller since timer 0 is allocated to delay () function and timer1 is allocated to servo motor. MsTimer2.h header file gives access to use timer 2. Servo.h is used in servo motor. SPI.h and LiquidCrystal.h are used in LCD display. SPI.h is the header file which is used in serial communication tasks.by using it we can use a modified version of LiquidCrystal.h file in programs.

       

     

variables used in program

     x is the variable that is being used to store the distance measurements by distance sensor. It updates value of x every 100ms or any other given time period using timer2 interrupts. It is declared as volatile since x gets updated outside of the program. Therefore it indicates to the program that value of x can be changed anytime.

direc is variable type that is constructed exclusively for this program using structures in C. structures allows programmer to construct user-defined data types that can be used to store various types of data. In here my requirement was to have a data type which is capable of storing both angle and distance to an obstacle from robot position. In other words I wanted to use polar coordinates to detect obstacles. My initial idea was to use two different arrays to store the angle and the distance separately but it didn’t sound good because these two parameters are actually related to each other. Therefore I decided to use a user-defined data type to store these two parameters. After constructing the data type you can use it to declare variables, just as you declare an integer or any other default data types.  

Structure used in program as a user-defined data type

          In here I have declared an array called data which has 7 elements in it. Each element can store both angle and direction since their data type is direc. Rest of the variables are conventional variables that are being used to store various parameters used by program.

The next step was to initialize the setup() function. Setup() function is used to assign pins in Arduino board to various inputs or outputs that are used in program. pinMode() function is used to assign an Arduino GPIO to an input (ex: echopin which is used to measure reflected ultra sound wave) or to an output (ex: trigpin which is used to initiate an ultra sound wave) . myservo.attach () does the same.In here myservo is an object that is declared as a Servo.

      New thing in setup () is the way that timer interrupt has been set up. Previously I was able to setup a timer interrupt from scratch but this time I used a function that was built by Arduino community to setup a timer interrupt. It made my life so easy and all I had to do was to give a value to the timer interrupt period and an ISR to be called when timer overflows. Flash() is the ISR and inside flash() I have called measure_distance() function. Therefore when ISR is called by timer interrupt every 100ms, it will call measure_distance() that will measure the distance to an obstacle. MsTimer2.h header file is responsible for handling timer 2. You can use this link to learn more about this.  

      

     

Setup() function

      Lcd_home() is used to initiate the lad display by clearing up the display and positioning the cursor on left upper corner.

Myservo.write (ini_angle) is used to make sure that sensor is facing forward direction when robot is turned on. The ini_angle varible value could be any value but my case it was around 70 degrees.

Now we’ll look at the functions I have used to in my program.    

Functions associated with robot movements

These functions will depend on the type of robot you are going to build. In my case it was a robot with wheels therefore it is expected that it should be able to move forward, turn left or right and backward. There are 4 connectors that need to be energised in order to rotate the motors. So I had to play with my robot a bit to identify what combination of these will make my robot go forward, backward, turn left and right. It’s something I had to do with trial and error. The most important thing to remember is to never set all motor connectors to high state since it will destroy the H bridge circuit in L293D IC.

functions that control movements of robots

     I’m not going to explain turn_robot() function here since it requires some other functions that yet to be explained. So I will present this function later in this post.

Functions associated with distance sensor

Measure_distance() is the function that measure the distance to an obstacle and store it in variable in every 100ms. It’s one of the most critical functions. It generates an ultrasound wave and listen to it’s reflection. Then it measures time delay between sending the signal and receiving it back. This value is then used to calculate x value. 

 

duration is the time delay measured and diving it by 58.2 this value is converted to a distance is cm.The speed of sound is 340 m/s or 29.1 microseconds per cm. duration is the time for wave to return to the sensor after reflection. Therefore duration value is halved when calculating distance. Since x is a global variable any other function can access this x value.

Function to measure displacement

Sort() function is used to arrange collected distance and angle data from smallest value to largest value. It takes the measured values that are stored in array called data and compare an element with it’s previous value. If the previous value is higher than the chosen element it will copy the previous value to another allocated memory place temporally. Then it moves the selected element to the position of previous element .Then copies the values in temporary memory into the next element. This process is done until all the values have been compared with it’s adjacent element. The following chart will explain it better.

The technique that I used in here is called nested for loop. It means a for loop inside another for loop. These types of loops are very useful when you are trying to manipulate things like 2dimentional arrays or matrices. In this case the inner most loop is the loop responsible of carrying out the comparison and copying elements. Outer most for loop controls the iterations occurred therefore the inner most loop.

Flow chart for sorting algorithm

Function used to arrange data in ascending order

I hope to explain the rest of the function from my next post.