DFRobot Devastator Tank Robot Part 2 Raspberry Pi Python Code
In Part 1 of this build, we covered parts and assembly of the DFRobot Devastator Tank Robot. Now we’ll go over some of the Python Code we’re running on the Raspberry Pi with the RaspiRobot Motor Driver Board.
You could definitely do this with another motor driver board, but you would need to remove the code I used for the RaspiRobot and replace with the code for the board you are using.
*Disclaimer: Most if not all links on this page will be Affiliate Links. You still pay the same price but it helps me pay for things like website hosting and more tech & gadgets to write about.
**Update: We followed Phil Martin’s excellent article USING YOUR NEW RASPBERRY PI 3 AS A WIFI ACCESS POINT WITH HOSTAPD and we are now getting much better FPV performance. We may try upgrading to a USB WiFi Dongle with an Antenna to get even more range out of it.
Raspberry Pi Setup
I did not use NOOBS, but instead I installed the latest version of Jesse Lite for this build. You can download that image here: https://downloads.raspberrypi.org/raspbian_lite_latest
Read the Raspberry Pi guide on how to install an image on to a micro sd card depending on your computer’s Operating System: https://www.raspberrypi.org/documentation/installation/installing-images/README.md
Adafruit wrote a great guide on how to get WiFi setup on the Raspberry Pi and you will need WiFi enabled for this project: https://learn.adafruit.com/adafruits-raspberry-pi-lesson-3-network-setup/setting-up-wifi-with-occidentalis
With all that done you can SSH into your Raspberry Pi and continue the setup.
Install Python & Git
sudo apt-get update sudo apt-get install python-pip python-dev build-essential python-serial sudo apt-get install git
Installing the RaspiRobot Motor Driver Code
Go to the RaspiRobot Github page here: https://github.com/simonmonk/raspirobotboard3
Use the alternative install method:
$ cd ~ $ git clone https://github.com/simonmonk/raspirobotboard3.git $ cd raspirobotboard3/python $ sudo python setup.py install
You will find some examples in there you can run. (Pro tip: Do not leave your Robot near the top of stairs while testing!)
Autonomous Mode
The RaspiRobot V3 motor board from Monk Makes conveniently has a socket for a HC-SR04 Ultrasonic Distance Sensor
Close Up of RaspiRobot Mounted on Raspberry Pi with Distance Sensor
In the video below our Devastator is running in “Autonomous Mode” or maybe more appropriately “Semi-Autonomous Mode”. I say semi because we could use another distance sensor on the rear and probably some distance sensors on the sides to help detect walls & objects better.
import rrb3 as rrb import time, random # Change these for your setup. BATTERY_VOLTS = 7.2 MOTOR_VOLTS = 6 # Configure the RRB rr = rrb.RRB3(BATTERY_VOLTS, MOTOR_VOLTS) # if you don't have a switch, change the value below to True running = True def turn_randomly(): turn_time = random.randint(1, 2) if random.randint(1, 2) == 1: rr.left(turn_time, 0.5) # turn at half speed rr.set_led1(1) rr.set_led1(0) #rr.set_motors(0.2, 0, 1, 1) rr.set_led1(1) time.sleep(turn_time) #rr.set_motors(0.3, 1, 1, 0) #time.sleep(1) rr.set_led1(0) else: rr.right(turn_time, 0.5) rr.set_led2(1) rr.set_led2(0) #rr.set_motors(0.2, 0, 1, 0) rr.set_led2(1) #time.sleep(turn_time) #rr.set_motors(0.3, 1, 1, 1) time.sleep(1) rr.set_led2(0) rr.stop() try: while True: distance = rr.get_distance() print(distance) if distance < 45 and running: rr.set_led2(1) rr.set_led1(1) rr.set_led1(0) rr.set_led2(0) turn_randomly() if running: #rr.forward() rr.set_motors(0.8, 0, 0.8, 0) rr.set_led2(1) rr.set_led1(1) if rr.sw2_closed(): running = not running if not running: rr.stop() time.sleep(0.2) finally: print("Exiting") rr.cleanup()
Copy this code and create a file called “auto-rover.py” in your /home/pi folder.
Then run sudo python /home/pi/auto-rover.py
You will see the distance output in your terminal screen and hopefully you robot will navigate way around your room for a while or until the battery runs out.
FPV Mode (First Person Video)
The next piece of code we run is called FPV or First Person Video. With FPV we will use a smartphone,tablet or computer to control our Devastator Robot via wifi. If you have a compatible Raspberry Pi camera you will be able to stream the video as well!
To make this happen, we will modify the Dexter Industries GoPiGo code which turns out to be modified Dawn Robotics code.
sudo apt-get update sudo pip install tornado ino cd ~ git clone https://github.com/DexterInd/GoPiGo.git cd ~/GoPiGo/Software/Python/Examples/Browser_Streaming_Robot
Now this next command will take quite a while to run, so be patient! Your Raspberry PI will reboot when this part of the install is finished.
sudo bash browser_stream_setup.sh
When that is finally done, you will need to modify a few files in the /GoPiGo/Software/Python/Examples/Browser_Streaming_Robot folder
cd ~/GoPiGo/Software/Python/Examples/Browser_Streaming_Robot
Let’s make backups of the original files:
mv camera_streamer.py camera_streamer.org mv robot_controller.py robot_controller.org mv robot_web_server.py robot_web_server.org
Now create the new camera_stream.py file:
nano camera_streamer.py
Put the following code in and save:
#!/usr/bin/env python ############################################################################################################## # This example is for streaming video and controlling the GoPiGo from a web browser # http://www.dexterindustries.com/GoPiGo/ # History # ------------------------------------------------ # Author Date Comments # Karan 24 July 14 Initial Authoring ''' ## License GoPiGo for the Raspberry Pi: an open source robotics platform for the Raspberry Pi. Copyright (C) 2015 Dexter Industries This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/gpl-3.0.txt>. ''' # # This example is derived from the Dawn Robotics Raspberry Pi Camera Bot # https://bitbucket.org/DawnRobotics/raspberry_pi_camera_bot ############################################################################################################# # Copyright (c) 2014, Dawn Robotics Ltd # All rights reserved. # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # 1. Redistributions of source code must retain the above copyright notice, this # list of conditions and the following disclaimer. # 2. Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # 3. Neither the name of the Dawn Robotics Ltd nor the names of its contributors # may be used to endorse or promote products derived from this software without # specific prior written permission. # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. import copy import os import os.path import subprocess import time import logging #--------------------------------------------------------------------------------------------------- class CameraStreamer: """A class to look after streaming images from the Raspberry Pi camera. Ideally, the camera should only be on when somebody wants to stream images. Therefore, startStreaming must be called periodically. If startStreaming is not called before a timeout period expires then the streaming will stop""" DEFAULT_TIMEOUT = 4.0 #----------------------------------------------------------------------------------------------- def __init__( self, timeout=DEFAULT_TIMEOUT ): self.cameraStreamerProcess = None self.streamingStartTime = 0 self.streamingTimeout = timeout #----------------------------------------------------------------------------------------------- def __del__( self ): self.stopStreaming() #----------------------------------------------------------------------------------------------- def startStreaming( self ): # Start raspberry_pi_camera_streamer if needed if self.cameraStreamerProcess == None or self.cameraStreamerProcess.poll() != None: self.cameraStreamerProcess = subprocess.Popen( [ "/usr/local/bin/raspberry_pi_camera_streamer" ] ) self.streamingStartTime = time.time() #----------------------------------------------------------------------------------------------- def update( self ): if time.time() - self.streamingStartTime > self.streamingTimeout: self.stopStreaming() #----------------------------------------------------------------------------------------------- def stopStreaming( self ): if self.cameraStreamerProcess != None: self.cameraStreamerProcess.terminate()
Now create the new robot_controller.py
nano robot_controller.py
Put the following code in and save:
#! /usr/bin/env python ############################################################################################################## # This example is for streaming video and controlling the GoPiGo from a web browser # http://www.dexterindustries.com/GoPiGo/ # History # ------------------------------------------------ # Author Date Comments # Karan 24 July 14 Initial Authoring # Karan 19 Feb 15 Converted to 1 joystick mode ''' ## License GoPiGo for the Raspberry Pi: an open source robotics platform for the Raspberry Pi. Copyright (C) 2015 Dexter Industries This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/gpl-3.0.txt>. ''' # # This example is derived from the Dawn Robotics Raspberry Pi Camera Bot # https://bitbucket.org/DawnRobotics/raspberry_pi_camera_bot ############################################################################################################# # Copyright (c) 2014, Dawn Robotics Ltd # All rights reserved. # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # 1. Redistributions of source code must retain the above copyright notice, this # list of conditions and the following disclaimer. # 2. Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # 3. Neither the name of the Dawn Robotics Ltd nor the names of its contributors # may be used to endorse or promote products derived from this software without # specific prior written permission. # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. import logging import math import time import Queue import threading import rrb3 as rrb #import gopigo #--------------------------------------------------------------------------------------------------- debug=1 BATTERY_VOLTS = 7.2 MOTOR_VOLTS = 6 # Configure the RRB rr = rrb.RRB3(BATTERY_VOLTS, MOTOR_VOLTS) class RobotController: MAX_UPDATE_TIME_DIFF = 0.25 TIME_BETWEEN_SERVO_SETTING_UPDATES = 1.0 JOYSTICK_DEAD_ZONE = 0.1 MOTION_COMMAND_TIMEOUT = 2.0 # If no commands for the motors are recieved in this time then # the motors (drive and servo) are set to zero speed speed_l=200 speed_r=200 #----------------------------------------------------------------------------------------------- def __init__( self ): #gopigo.set_speed(200) #gopigo.stop() #gopigo.fwd() rr.stop() self.lastServoSettingsSendTime = 0.0 self.lastUpdateTime = 0.0 self.lastMotionCommandTime = time.time() #----------------------------------------------------------------------------------------------- def __del__( self ): self.disconnect() #----------------------------------------------------------------------------------------------- def disconnect( self ): print "Closing" def normaliseJoystickData( self, joystickX, joystickY ): stickVectorLength = math.sqrt( joystickX**2 + joystickY**2 ) if stickVectorLength > 1.0: joystickX /= stickVectorLength joystickY /= stickVectorLength if stickVectorLength < self.JOYSTICK_DEAD_ZONE: joystickX = 0.0 joystickY = 0.0 return ( joystickX, joystickY ) def centreNeck( self ): #gopigo.set_right_speed(0) pass def setMotorJoystickPos( self, joystickX, joystickY ): joystickX, joystickY = self.normaliseJoystickData( joystickX, joystickY ) if debug: print "Left joy",joystickX, joystickY #print self.speed_l*joystickY #gopigo.set_left_speed(int(self.speed_l*joystickY)) #gopigo.fwd() if joystickX > .5: print "Left" #gopigo.left() rr.left(); elif joystickX .5: print "Fwd" #gopigo.fwd() rr.forward() elif joystickY < -.5: print "Back" #gopigo.bwd() rr.reverse() else: print "Stop" #gopigo.stop() rr.stop() def setNeckJoystickPos( self, joystickX, joystickY ): #print "g" joystickX, joystickY = self.normaliseJoystickData( joystickX, joystickY ) if debug: print "Right joy",joystickX, joystickY #print self.speed_r*joystickY #gopigo.set_right_speed(int(self.speed_r*joystickY)) #gopigo.fwd() #self.lastMotionCommandTime = time.time() def update( self ): if debug: print "Updating" curTime = time.time() timeDiff = min( curTime - self.lastUpdateTime, self.MAX_UPDATE_TIME_DIFF ) # Turn off the motors if we haven't received a motion command for a while #if curTime - self.lastMotionCommandTime > self.MOTION_COMMAND_TIMEOUT: # self.leftMotorSpeed = 0.0 # self.rightMotorSpeed = 0.0 # self.panSpeed = 0.0 # self.tiltSpeed = 0.0 self.lastUpdateTime = curTime
Now create robot_web_server.py
#! /usr/bin/env python ############################################################################################################## # This example is for streaming video and controlling the GoPiGo from a web browser # http://www.dexterindustries.com/GoPiGo/ # History # ------------------------------------------------ # Author Date Comments # Karan 24 July 14 Initial Authoring ''' ## License GoPiGo for the Raspberry Pi: an open source robotics platform for the Raspberry Pi. Copyright (C) 2015 Dexter Industries This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/gpl-3.0.txt>. ''' # # This example is derived from the Dawn Robotics Raspberry Pi Camera Bot # https://bitbucket.org/DawnRobotics/raspberry_pi_camera_bot ############################################################################################################ # Copyright (c) 2014, Dawn Robotics Ltd # All rights reserved. # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # 1. Redistributions of source code must retain the above copyright notice, this # list of conditions and the following disclaimer. # 2. Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # 3. Neither the name of the Dawn Robotics Ltd nor the names of its contributors # may be used to endorse or promote products derived from this software without # specific prior written permission. # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. import logging LOG_FILENAME = "/tmp/robot_web_server_log.txt" # Check Below Path Matches Your Location of Files file_location = "/home/pi/GoPiGo/Software/Python/Examples/Browser_Streaming_Robot/www/" logging.basicConfig( filename=LOG_FILENAME, level=logging.DEBUG ) # Also log to stdout consoleHandler = logging.StreamHandler() consoleHandler.setLevel( logging.DEBUG ) logging.getLogger( "" ).addHandler( consoleHandler ) import os.path import math import time import signal import tornado.httpserver import tornado.ioloop import tornado.web import tornado.escape import sockjs.tornado import threading import Queue import camera_streamer import robot_controller import json #import gopigo import rrb3 as rrb import subprocess import sys robot = None cameraStreamer = None scriptPath = os.path.dirname( __file__ ) webPath = os.path.abspath( file_location) print webPath robotConnectionResultQueue = Queue.Queue() isClosing = False #--------------------------------------------------------------------------------------------------- def createRobot(resultQueue ): r = robot_controller.RobotController( ) resultQueue.put( r ) #--------------------------------------------------------------------------------------------------- class ConnectionHandler( sockjs.tornado.SockJSConnection ): #----------------------------------------------------------------------------------------------- def on_open( self, info ): pass #----------------------------------------------------------------------------------------------- def on_message( self, message ): try: message = str( message ) except Exception: logging.warning( "Got a message that couldn't be converted to a string" ) return if isinstance( message, str ): lineData = message.split( " " ) if len( lineData ) > 0: if lineData[ 0 ] == "Centre": if robot != None: robot.centreNeck() elif lineData[ 0 ] == "StartStreaming": cameraStreamer.startStreaming() elif lineData[ 0 ] == "Shutdown": cameraStreamer.stopStreaming() #gopigo.stop() rr.stop() robot.disconnect() sys.exit() elif lineData[ 0 ] == "Move" and len( lineData ) >= 3: motorJoystickX, motorJoystickY = \ self.extractJoystickData( lineData[ 1 ], lineData[ 2 ] ) if robot != None: robot.setMotorJoystickPos( motorJoystickX, motorJoystickY ) elif lineData[ 0 ] == "PanTilt" and len( lineData ) >= 3: neckJoystickX, neckJoystickY = \ self.extractJoystickData( lineData[ 1 ], lineData[ 2 ] ) if robot != None: robot.setNeckJoystickPos( neckJoystickX, neckJoystickY ) #----------------------------------------------------------------------------------------------- def on_close( self ): logging.info( "SockJS connection closed" ) def extractJoystickData( self, dataX, dataY ): joystickX = 0.0 joystickY = 0.0 try: joystickX = float( dataX ) except Exception: pass try: joystickY = float( dataY ) except Exception: pass return ( joystickX, joystickY ) #--------------------------------------------------------------------------------------------------- class MainHandler( tornado.web.RequestHandler ): #------------------------------------------------------------------------------------------------ def get( self ): self.render( webPath + "/index.html" ) #--------------------------------------------------------------------------------------------------- def robotUpdate(): global robot global isClosing if isClosing: tornado.ioloop.IOLoop.instance().stop() return if robot == None: if not robotConnectionResultQueue.empty(): robot = robotConnectionResultQueue.get() else: robot.update() #--------------------------------------------------------------------------------------------------- def signalHandler( signum, frame ): if signum in [ signal.SIGINT, signal.SIGTERM ]: global isClosing isClosing = True #--------------------------------------------------------------------------------------------------- if __name__ == "__main__": signal.signal( signal.SIGINT, signalHandler ) signal.signal( signal.SIGTERM, signalHandler ) # Create the configuration for the web server router = sockjs.tornado.SockJSRouter( ConnectionHandler, '/robot_control' ) application = tornado.web.Application( router.urls + [ ( r"/", MainHandler ), ( r"/(.*)", tornado.web.StaticFileHandler, { "path": webPath } ), ( r"/css/(.*)", tornado.web.StaticFileHandler, { "path": webPath + "/css" } ), ( r"/css/images/(.*)", tornado.web.StaticFileHandler, { "path": webPath + "/css/images" } ), ( r"/images/(.*)", tornado.web.StaticFileHandler, { "path": webPath + "/images" } ), ( r"/js/(.*)", tornado.web.StaticFileHandler, { "path": webPath + "/js" } ) ] ) #( r"/(.*)", tornado.web.StaticFileHandler, {"path": scriptPath + "/www" } ) ] \ # Create a camera streamer cameraStreamer = camera_streamer.CameraStreamer() # Start connecting to the robot asyncronously robotConnectionThread = threading.Thread( target=createRobot, args=[ robotConnectionResultQueue ] ) #args=[ robotConfig, robotConnectionResultQueue ] ) robotConnectionThread.start() # Now start the web server logging.info( "Starting web server..." ) http_server = tornado.httpserver.HTTPServer( application ) #The port number on which the robot control works, change in line 105 in www/index.html too http_server.listen( 98 ) robotPeriodicCallback = tornado.ioloop.PeriodicCallback( robotUpdate, 100, io_loop=tornado.ioloop.IOLoop.instance() ) robotPeriodicCallback.start() cameraStreamerPeriodicCallback = tornado.ioloop.PeriodicCallback( cameraStreamer.update, 1000, io_loop=tornado.ioloop.IOLoop.instance() ) cameraStreamerPeriodicCallback.start() tornado.ioloop.IOLoop.instance().start() # Shut down code robotConnectionThread.join() if robot != None: robot.disconnect() else: if not robotConnectionResultQueue.empty(): robot = robotConnectionResultQueue.get() robot.disconnect() cameraStreamer.stopStreaming()
When you are done with those changes, run:
sudo python ~/GoPiGo/Software/Python/Examples/Browser_Streaming_Robot/robot_web_server.py
Then go to your computer, tablet or smartphone and enter the IP address of your Raspberry Pi into your browser like this:
Now, I will note I had some issues with the joystick commands hanging up. I do think this would work better with the Raspberry Pi acting as it’s own WiFi Access Point so it’s not dealing with the traffic from the rest of my network. Once I have that worked out, I will add it to this post. This will also allow you to control it better outside where there is no other WiFi.
Reminder, some links in the post might be affiliate links, so I’ll make some coffee/beer/gadget money if you buy anything. You get it at the normal price, though!