Anna The Multibot!

With a working voice on the robot, it is a good time to add a practical automatic function such as storytelling. With a voice-coordinated head motion and a display to display the words being spoken, a storytelling robot could be a great educational tool to teach reading skills to small children. I could also see other educational functions to teach math, science and history. Lots of robot kits are designed to teach engineering, but why not have a robot to help teach other subjects as well.

I added a new form to display the story. The form contains a “rich” textbox and 3 buttons for pause, resume and cancel. The display will display one sentence at a time as the robot tells the story so a child can read along.
The below images shows the display, the code to handle the buttons and what the display looks like while the robot is telling the story. In the future, the display will be on the robot:

To the “command class” I added the following code to decode the command to tell a story:
If command_input.Contains("tell") And command_input.Contains("story") Then
Speech.speak_text("ok...")
story()
End If
Also under “command" class I added the following sub to load the story and call the read sub.

Private Sub story()
Anna.Command_text.Text = Nothing
Anna.load_story()
Anna.read_text()
End Sub
In the main class I added the following subs to load the story, tell the story and handle buttons to pause, resume and cancel the story and also automate some head motion to keep the viewers’ attention:
Public Sub load_story()
Try
' Open the file using a stream reader.
Using sr As New StreamReader("C:\Anna\Anna_Robot_Project\Anna\story\jack and the beanstalk.txt")
Dim line As String
' Read the stream to a string and write the string to the console.
line = sr.ReadToEnd()
s = line
End Using
Catch

End Try
'TextBox1.Text = s
End Sub
Public Sub read_text()
Dim first_index As Integer
Dim second_index As Integer
Dim length As Integer
Dim segment As String = Nothing
Dim myForm2 As New Display()
first_index = -1

display_cancel = False
Do
second_index = (s.IndexOf(".", second_index + 1))
length = second_index - (first_index)
If length < 1 Then Exit Do
segment = s.Substring(first_index + 1, length)
myForm2.Show()
myForm2.RichTextBox1.Text = segment
first_index = second_index
speech1(segment)
If second_index > s.Length - 1 Then Exit Do
Dim a As String = segment.Length
If display_cancel = True Then Exit Do
Loop
myForm2.Close()
End Sub
Private Sub speech1(st As String)
synthesizer.SelectVoiceByHints(VoiceGender.Female, VoiceAge.Adult)
speaker.Rate = 0
speaker.Volume = 100
AddHandler speaker.SpeakCompleted, AddressOf SpeachComplete
speaker.SpeakAsync(st)
speech_head_motion()
While speach_done = False
Application.DoEvents()
If display_cancel = True Then Exit While
End While
speach_done = False
End Sub
Private Sub SpeachComplete(sender As Object, e As SpeakCompletedEventArgs)
speach_done = True
End Sub
Public Sub speach_pause()
speaker.Pause()
Speech.speak_text("Story is paused")
End Sub
Public Sub speach_resume()
Speech.speak_text("Story is resuming")
Threading.Thread.Sleep(500)
speaker.Resume()
End Sub
Public Sub speach_stop()
display_cancel = True
speaker.SpeakAsyncCancelAll()
Speech.speak_text("Story is terminated")
End Sub
Private Sub speech_head_motion()
head_up(800)
head_speech_count = head_speech_count + 3
If head_speech_count > 10 Then head_speech_count = 0
If head_speech_count > 8 Then head_forward()
If head_speech_count > 8 Then head_up(500)
If head_speech_count > 5 And head_speech_count < 8 Then head_left(500)
If head_speech_count < 5 Then head_right(500)
End Sub

Here’s two automated functions added to Anna. Face detect and canny frame. Both are called by plain text commands and both have a voice response. Below the video is a breakdown of the steps.

Now with a way to call different automated processes from the last log, it’s time to add some processes. I thought I would start off with some OpenCV functions. Luckily with a wrapper like EMGU, adding OpenCV is fairly easy. To automate the process, we need a process to call, a timer, points to check to see if the process is being requested and a way to make the point active.
I added a new class to contain the OpenCV functions called "OpenCV_functions". Within the class, I added routines for face detection and canny frame (more to be added later).
Two Boolean points are also added to the main page. One is called “face” and one is called “canny_frame”.
Public canny_frame As Boolean = False
Public face As Boolean = False
Both are Public to allow them to be set/cleared from other classes and they are both initialized as false.
In the “command” class under the “text_decode” sub I added the following to lines of code:
If command_input.Contains("show") Then show()
If command_input.Contains("cancel") Then cancel()
Also in the command class I added the following subs:

Public Sub show()
If command_input.Contains("canny") And command_input.Contains("frame") Then
Machine_vision.canny_frame = True
Dim Thread2 As New Thread(Sub() Speech.speak_text("Canny frame is now active, by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("face") And command_input.Contains("detect") Then
Machine_vision.face = True
Dim Thread2 As New Thread(Sub() Speech.speak_text("face detect is now active, by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
End Sub
Public Sub cancel()
If command_input.Contains("canny") And command_input.Contains("frame") Then
Machine_vision.canny_frame = False
Dim Thread2 As New Thread(Sub() Speech.speak_text("Canny frame is now inactive, by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("face") And command_input.Contains("detect") Then
Machine_vision.face = False
Dim Thread2 As New Thread(Sub() Speech.speak_text("face detect is now inactive, by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
End Sub

Last I added a timer to the main page:
Private Sub Timer1_Tick(sender As Object, e As EventArgs) Handles Timer1.Tick
load_jpg()
PictureBox1.Image = picture
If canny_frame = True Then Opencv_functions.canny()
If face = True Then Opencv_functions.face_detect()
End Sub
The load() loads the picture from the camera to proccess and if the Boolean point is set the timer will call it. The Boolean points are set and cleared in the command class based on text commands.
Below is the OpenCV functions:

Public Sub face_detect()
faces = Nothing
face_count = 1
Try

Dim ColordImage As Image(Of Bgr, [Byte]) = New Image(Of Bgr, Byte)(picture)
'Load the object detector
Dim faceDetector As New CascadeClassifier("haarcascade_frontalface_default.xml")
'Convert the image to Grayscale
Dim grayImage As Image(Of Gray, [Byte]) = ColordImage.Convert(Of Gray, [Byte])()
For Each face As Rectangle In faceDetector.DetectMultiScale(
grayImage,
1.1,
10,
New Size(25, 20),
Size.Empty)
CvInvoke.Rectangle(ColordImage, face, New MCvScalar(0, 0, 255))
Dim p As PointF = New PointF
p = face.Location
Dim size As String = face.Size.ToString
faces = faces + ((" face" & face_count & vbLf + (" X: " _
+ (p.X.ToString + (" Y: " + p.Y.ToString))) + " " + size + ";"))
face_count = face_count + 1
Next
Dim MyImage As Image = ColordImage.ToBitmap()
Machine_vision.PictureBox1.Image = MyImage
Catch
End Try
End Sub

Public Sub canny()
Try
Dim cannyFrame As New Mat()
Dim cannyFrame2 As New Mat()
Dim ColordImage As Image(Of Bgr, [Byte]) = New Image(Of Bgr, Byte)(picture)
Dim grayImage As Image(Of Gray, [Byte]) = ColordImage.Convert(Of Gray, [Byte])()
Dim pyrDown As New UMat()
CvInvoke.PyrDown(grayImage, pyrDown)
CvInvoke.PyrUp(pyrDown, grayImage)
CvInvoke.Canny(grayImage, cannyFrame, 100, 60)
Dim img As Image(Of Bgr, [Byte]) = cannyFrame.ToImage(Of Bgr, [Byte])()
Dim MyImage As Image = img.ToBitmap()
Machine_vision.PictureBox1.Image = MyImage
Catch
End Try
End Sub

To have a robot with a lot of different functions, it is necessary to come up with a way to call the different functions. Ideally, any one off the street could be able to tell the robot to perform a task. On that thought, plain text commands seem the most obvious way to accomplish this, and a voice response also seems like a good idea.
To add very complex capabilities with lots of functions to any project, I like to add a new class to house the functions and subs. It makes the program a bit easier to follow and keeps it looking neater.
On that note, I added two new classes to the Visual Studios program “command and speech.”
I added a “command” text box to receive the commands. As text is typed in the text box, the command class is called as each character is typed to determine if the command matches any of the command keywords with the below sub.

Private Sub Command_TextChanged(sender As Object, e As EventArgs) Handles Command_text.TextChanged, Command_text.TextChanged
command_input = Command_text.Text
Command.text_decode()

End Sub

The Speech Class at the moment just contains one sun that speaks a string of text that is send when the sub is called. Below is the speech class.

Imports System.Speech.Synthesis
Public Class Speech
Private speaker As New SpeechSynthesizer()

Public Sub speak_text(text As String)
Dim speech
speech = CreateObject("bot.voice")
speech.speak(text)
End Sub
End Class

The Command class determines if any command key word is typed and if one or a combination of keywords is typed it responds with a voice response and calls the operation. The command class at the moment calls the robots motor and head commands. It also opens a second thread for the voice so both subs run at the same time.

Imports System.Threading

Public Class Command
Dim Speech As New Speech
Public Sub text_decode()
If command_input.Contains("look") Then headcommand()
If command_input.Contains("head") Then headcommand()
If command_input.Contains("move") Then movecommand()
If command_input.Contains("turn") Then movecommand()
If command_input.Contains("backup") Then movecommand()
If command_input.Contains("back up") Then movecommand()
End Sub
Public Sub headcommand()
If command_input.Contains("up") Then
Machine_vision.head_up(1500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("looking up by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("down") Then
Machine_vision.head_down(1500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("looking down by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("forward") Then
Machine_vision.head_forward()
Dim Thread2 As New Thread(Sub() Speech.speak_text("looking forward by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("left") Then
Machine_vision.head_left(1500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("looking left by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("right") Then
Machine_vision.head_right(1500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("looking right by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("stop") Then
Machine_vision.drive_stop()
Dim Thread2 As New Thread(Sub() Speech.speak_text("stop by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
End Sub
Public Sub movecommand()
If command_input.Contains("forward") Then
Machine_vision.drive_forward(1500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("moveing forward by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("turn left") Then
Machine_vision.drive_left_f(500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("turning left by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("turn right") Then
Machine_vision.drive_right_f(500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("turning right by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("back up") Then
Machine_vision.drive_reverse(500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("backing up by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("backup") Then
Machine_vision.drive_reverse(1500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("backing up by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("rotate left") Then
Machine_vision.drive_left(500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("rotateing left by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
If command_input.Contains("rotate right") Then
Machine_vision.drive_right(500)
Dim Thread2 As New Thread(Sub() Speech.speak_text("rotateing right by your command"))
Machine_vision.Command_text.Text = Nothing
Thread2.Start()
End If
End Sub

End Class

We now have a way to call different Automated processes. Just add the if statement with the word and call the process.

Below is a screen shot of the command text box.

Below is Anna receiving text commands with voice response.

A quick trip around the kitchen testing out the object avoidance vision system. The robot is turning based on left side or right side threats. Fairly simple: If right side threat then left turn routine. If left side threat then right turn routine.

The program at this point has both a canny image and the original colored bitmap the canny image was derived from. With a canny edge image, we can now start getting information on where the objects are in the image.
The technique I used is to start at the bottom of the canny image and start checking to see if the pixels are black or white. A black pixel means no edge and a white pixel means an edge is detected at that location. If a black pixel is detected, the program moves up the image to the next pixel and continues to check pixels until it has either detected a white pixel or it has reached the top of the image.
If a white pixel is detected, the program stores its data in an array which includes the white pixel’s x and y location from the bottom of the image. As the program is checking the pixels, it is also calculating the average hue and brightness from the colored bitmap of the areas that are detected not to be edges. This information is also stored in the array and will give us additional useful data for later. It is not necessary to map every line of pixels, so I’m only mapping every 10th line. This cuts down on processor time.
This allows detecting of horizontal edges. More important than horizontal edges is vertical edges. A horizontal edge could be just a line on the floor but a vertical line could be a wall. After the above section of code has ran, two more sections run to detect vertical edges.
The canny image is divided into a left and a right side. From a line in the center of the image, a section of code checks the left side of the image for edges from the center of the image to the far left side. The same is also performed on the right side of the image. The information is also stored in arrays for further analysis.
There is a short section of code to redraw the lines mapped. On the image below, red lines are the mapped lines from the bottom on the image, and blue lines are the vertical lines from the center of the image to the left and right side of the image.

Below is the sub I used to detect the lines. The arrays are defined in a module as PUBLIC to allow different sections of the program to use the data. I added a clase for vision to separate the code into sections.

Public Sub edge_range()
Dim color As Color
Dim hue As Integer = Nothing
Dim brightness As Integer
Dim x As Integer
Dim y As Integer
Dim color_hue As Color
Dim color_lum As Integer = Nothing
Dim image1 As Bitmap = Machine_vision.edges()
Dim image_color As Bitmap = Machine_vision.picture
Dim i As Integer
Dim object_location As Integer
Dim a As Integer = 0
Dim line_count As Integer = 0
Dim longest_x As Integer = 0
Dim longest_y As Integer = 0
x = 5
y = 479
For a = 0 To 62
Do Until y = 0
color = image1.GetPixel(x, y)
color_hue = image_color.GetPixel(x, y)
brightness = color.GetBrightness
If brightness = 1 Then
object_location = y
Exit Do
Else
y = y - 1
If hue = Nothing Then hue = color_hue.GetHue Else hue = (color_hue.GetHue + hue) / 2
If color_lum = Nothing Then color_lum = color_hue.GetBrightness * 10 Else color_lum = ((color_hue.GetBrightness * 10) + color_lum) / 2
End If
Loop
range(a, 0, 0) = 480 - object_location
If a + 1 <> 63 Then range(a + 1, 0, 0) = Nothing
range(a, 1, 0) = x
range(a, 2, 0) = hue
range(a, 3, 0) = color_lum
If a = 0 Then
range(a, 4, 0) = 1 : line_count = 1
range(a, 5, 0) = 0
range(a, 6, 0) = 0
ElseIf Math.Abs(range(a, 0, 0) - range(a - 1, 0, 0)) < 10 Then
range(a, 4, 0) = line_count
Else
line_count = line_count + 1
range(a, 4, 0) = line_count
End If

If 480 - object_location > longest_y Then longest_y = 480 - object_location : longest_x = range(a, 1, 0)
y = 479
x = x + 10

Next

x = 5
For a = 1 To 62
For i = 480 - range(a, 0, 0) To 479
Machine_vision.picture.SetPixel(range(a, 1, 0), i, Color.FromArgb(255, 0, 0))

Next
Next

'***********************look for wall edges **************************
Dim line_count1 As Integer = 0
Dim line_count2 As Integer = 0
y = 475
a = 0
Do Until y < 10
x = 320
color = image1.GetPixel(x, y)
color_hue = image_color.GetPixel(x, y)
vertical_lines(a, 3) = color_hue.GetHue
Do Until x < 5
color = image1.GetPixel(x, y)
color_hue = image_color.GetPixel(x, y)
hue = color_hue.GetHue
color_lum = color_hue.GetBrightness * 10
color_hue = image_color.GetPixel(x, y)
brightness = color.GetBrightness

If brightness = 1 Then
Exit Do
Else
If hue = Nothing Then hue = color_hue.GetHue Else hue = (color_hue.GetHue + hue) / 2
If color_lum = Nothing Then color_lum = color_hue.GetBrightness * 10 Else color_lum = ((color_hue.GetBrightness * 10) + color_lum) / 2
x = x - 1

End If
Loop
vertical_lines(a, 0) = 480 - y
vertical_lines(a + 1, 0) = Nothing
vertical_lines(a, 1) = 320 - x
If a = 0 Then
vertical_lines(a, 5) = 1 : line_count1 = 1
vertical_lines(a, 6) = 0
vertical_lines(a, 7) = 0
ElseIf Math.Abs(vertical_lines(a, 1) - vertical_lines(a - 1, 1)) < 10 Then
vertical_lines(a, 5) = line_count1
Else
line_count1 = line_count1 + 1
vertical_lines(a, 5) = line_count1
End If

'***********************

x = 320
Do Until x > 635
color = image1.GetPixel(x, y)
'color_hue = image_color.GetPixel(x, y)
brightness = color.GetBrightness

If brightness = 1 Then
Exit Do
Else
If hue = Nothing Then hue = color_hue.GetHue Else hue = (color_hue.GetHue + hue) / 2
If color_lum = Nothing Then color_lum = color_hue.GetBrightness * 10 Else color_lum = ((color_hue.GetBrightness * 10) + color_lum) / 2
x = x + 1

End If
Loop
vertical_lines(a, 2) = x - 320
vertical_lines(a, 3) = hue
vertical_lines(a, 4) = color_lum
If a = 0 Then
vertical_lines(a, 8) = 1 : line_count2 = 1
vertical_lines(a, 9) = 0
vertical_lines(a, 10) = 0
ElseIf Math.Abs(vertical_lines(a, 2) - vertical_lines(a - 1, 2)) < 10 Then
vertical_lines(a, 8) = line_count2
Else
line_count2 = line_count2 + 1
vertical_lines(a, 8) = line_count2
End If

a = a + 1
y = y - 10
Loop

'*****************************************

For a = 1 To 96
If vertical_lines(a, 0) = Nothing Then Exit For

For i = 320 - vertical_lines(a, 1) To vertical_lines(a, 2) + 320
Machine_vision.picture.SetPixel(i, 480 - vertical_lines(a, 0), Color.Blue)
Next
Next

Machine_vision.PictureBox1.Image = Machine_vision.picture

End Sub

With the image from the camera now in the program, we need to look for object edges. This is one of the easiest ways to locate threats to the robot’s movements. An easy way to do this is to take the bitmap image that we have from the camera and invoke the canny function from the OpenCV library to implement the Canny Edge Detector. Below is the function I use to accomplish this. It returns a black and white image. The background is black and the edges show up white.

Public Function edges() As Bitmap
Dim cannyFrame As New Mat()
Dim cannyFrame2 As New Mat()
Dim ColordImage As Image(Of Bgr, [Byte]) = New Image(Of Bgr, Byte)(picture)
Dim grayImage As Image(Of Gray, [Byte]) = ColordImage.Convert(Of Gray, [Byte])()
Dim pyrDown As New UMat()
CvInvoke.Canny(grayImage, cannyFrame, 100, 60)
Dim img As Image(Of Bgr, [Byte]) = cannyFrame.ToImage(Of Bgr, [Byte])()
Dim MyImage As Image = img.ToBitmap()
Return img.ToBitmap()
End Function

Here's the image with the bitmap:

Here's the image after invoking the Canny Edge Detector:

First things first. In order to experiment with a vision system, you need a camera and you need to get the camera’s picture from the camera into a program. I configured the Tenvis camera to my local WIFI router. Follow the Tenvis easy setup in files.
I added a tutorial to demonstrate how to get the camera picture from the camera into the program. After running the tutorial, you should be able to display the camera’s picture in the form like the picture below. This is also a good first step to building a telepresence robot!

Here’s an overview of Anna inside and out. All in all a fairly simple robotics system. I originally built it to experiment with everything robotics.

And here's the inside.

Here’s a video of Anna under remote control from a Laptop through a WIFI network with a video feed from the robot’s camera and testing control of the robot’s camera pan and tilt, along with control of the robot’s two drive motors.