├── README.md └── pi2go ├── __init__.py ├── avoider.py ├── extra_tools ├── Adafruit_I2C.py ├── Adafruit_I2C.pyc ├── Adafruit_PWM_Servo_Driver.py ├── Adafruit_PWM_Servo_Driver.pyc ├── __init__.py ├── __init__.pyc ├── info.txt ├── line_following │ ├── sample_track.pdf │ └── track_generator.pdf ├── servod.xxx └── sgh_PCF8591P.pyc ├── line_follower.py ├── motor.py ├── pi2go.py └── pi2go.pyc /README.md: -------------------------------------------------------------------------------- 1 | Robots! 2 | ===== 3 | 4 | Robots are awesome. That is a fact. I am The Raspberry Pi Guy and I am going to show you how to build your first robot. That makes me awesome too. Deal with it ;-) 5 | 6 | This repo accompanies my robotics series which can be found here: http://youtu.be/_hDy9WY5UM0 7 | 8 | Thanks for watching and don't forget to subscribe, like and share. 9 | 10 | Matt 11 | 12 | The Raspberry Pi Guy 13 | 14 | theraspberrypiguy@gmail.com 15 | 16 | www.theraspberrypiguy.com 17 | 18 | www.youtube.com/theraspberrypiguy 19 | 20 | @RaspberryPiGuy1 -------------------------------------------------------------------------------- /pi2go/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /pi2go/avoider.py: -------------------------------------------------------------------------------- 1 | # Pi2Go 'avoider sketch' - for the second episode of my robot tutorial series 2 | # This program is fairly simple - it utilises the IR and ultrasonic sensors 3 | # on the Pi2Go in order to sense obstacles and avoid them 4 | # Created by Matthew Timmons-Brown and Simon Beal 5 | 6 | import pi2go, time 7 | 8 | pi2go.init() 9 | 10 | # Here we set the speed to 40 out of 100 - feel free to change! 11 | speed = 40 12 | 13 | # Here is the main body of the program - a lot of while loops and ifs! 14 | # In order to get your head around it go through the logical steps slowly! 15 | try: 16 | while True: 17 | if pi2go.irLeft(): 18 | while pi2go.irLeft(): 19 | # While the left sensor detects something - spin right 20 | pi2go.spinRight(speed) 21 | pi2go.stop() 22 | if pi2go.irRight(): 23 | while pi2go.irRight(): 24 | # While the right sensor detects something - spin left 25 | pi2go.spinLeft(speed) 26 | pi2go.stop() 27 | while not (pi2go.irLeft() or pi2go.irRight()): 28 | if pi2go.getDistance() <= 0.3: # If the distance is less than 0.3, spin right for 1 second 29 | pi2go.spinRight(speed) 30 | time.sleep(1) 31 | else: 32 | pi2go.forward(speed) 33 | pi2go.stop() 34 | 35 | finally: # Even if there was an error, cleanup 36 | pi2go.cleanup() 37 | -------------------------------------------------------------------------------- /pi2go/extra_tools/Adafruit_I2C.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # import smbus (Saves installing extra stuff for TRPG tutorials... If needed uncomment this and install smbus software + turn on i2c) 4 | 5 | # =========================================================================== 6 | # Adafruit_I2C Class mod @cymplecy 29Oct13 error raises exception 7 | # =========================================================================== 8 | 9 | class Adafruit_I2C : 10 | 11 | @staticmethod 12 | def getPiRevision(): 13 | "Gets the version number of the Raspberry Pi board" 14 | # Courtesy quick2wire-python-api 15 | # https://github.com/quick2wire/quick2wire-python-api 16 | try: 17 | with open('/proc/cpuinfo','r') as f: 18 | for line in f: 19 | if line.startswith('Revision'): 20 | return 1 if line.rstrip()[-1] in ['1','2','3'] else 2 # 3 added as also means Rev1 Board 21 | except: 22 | return 0 23 | 24 | @staticmethod 25 | def getPiI2CBusNumber(): 26 | # Gets the I2C bus number /dev/i2c# 27 | #print "ADAfruit Board Revision Detects:" , Adafruit_I2C.getPiRevision() 28 | return 1 if Adafruit_I2C.getPiRevision() > 1 else 0 29 | 30 | def __init__(self, address, busnum=-1, debug=False): 31 | self.address = address 32 | # By default, the correct I2C bus is auto-detected using /proc/cpuinfo 33 | # Alternatively, you can hard-code the bus version below: 34 | # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's) 35 | # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's) 36 | self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber()) 37 | self.debug = debug 38 | 39 | def reverseByteOrder(self, data): 40 | "Reverses the byte order of an int (16-bit) or long (32-bit) value" 41 | # Courtesy Vishal Sapre 42 | byteCount = len(hex(data)[2:].replace('L','')[::2]) 43 | val = 0 44 | for i in range(byteCount): 45 | val = (val << 8) | (data & 0xff) 46 | data >>= 8 47 | return val 48 | 49 | def errMsg(self): 50 | # Commented out by Matthew Timmons-Brown to stop annoying errors on the Pi2Go-Lite 51 | #print "Error accessing device at address 0x%02X" % self.address 52 | #raise Exception("Error accesing I2C Device") # Added by Simon Walters to raise an error if no device found 53 | return -1 54 | 55 | def write8(self, reg, value): 56 | "Writes an 8-bit value to the specified register/address" 57 | try: 58 | self.bus.write_byte_data(self.address, reg, value) 59 | if self.debug: 60 | print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg) 61 | except IOError, err: 62 | return self.errMsg() 63 | 64 | def write16(self, reg, value): 65 | "Writes a 16-bit value to the specified register/address pair" 66 | try: 67 | self.bus.write_word_data(self.address, reg, value) 68 | if self.debug: 69 | print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" % 70 | (value, reg, reg+1)) 71 | except IOError, err: 72 | return self.errMsg() 73 | 74 | def writeList(self, reg, list): 75 | "Writes an array of bytes using I2C format" 76 | try: 77 | if self.debug: 78 | print "I2C: Writing list to register 0x%02X:" % reg 79 | print list 80 | self.bus.write_i2c_block_data(self.address, reg, list) 81 | except IOError, err: 82 | return self.errMsg() 83 | 84 | def readList(self, reg, length): 85 | "Read a list of bytes from the I2C device" 86 | try: 87 | results = self.bus.read_i2c_block_data(self.address, reg, length) 88 | if self.debug: 89 | print ("I2C: Device 0x%02X returned the following from reg 0x%02X" % 90 | (self.address, reg)) 91 | print results 92 | return results 93 | except IOError, err: 94 | return self.errMsg() 95 | 96 | def readU8(self, reg): 97 | "Read an unsigned byte from the I2C device" 98 | try: 99 | result = self.bus.read_byte_data(self.address, reg) 100 | if self.debug: 101 | print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % 102 | (self.address, result & 0xFF, reg)) 103 | return result 104 | except IOError, err: 105 | return self.errMsg() 106 | 107 | def readS8(self, reg): 108 | "Reads a signed byte from the I2C device" 109 | try: 110 | result = self.bus.read_byte_data(self.address, reg) 111 | if result > 127: result -= 256 112 | if self.debug: 113 | print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % 114 | (self.address, result & 0xFF, reg)) 115 | return result 116 | except IOError, err: 117 | return self.errMsg() 118 | 119 | def readU16(self, reg): 120 | "Reads an unsigned 16-bit value from the I2C device" 121 | try: 122 | result = self.bus.read_word_data(self.address,reg) 123 | if (self.debug): 124 | print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) 125 | return result 126 | except IOError, err: 127 | return self.errMsg() 128 | 129 | def readS16(self, reg): 130 | "Reads a signed 16-bit value from the I2C device" 131 | try: 132 | result = self.bus.read_word_data(self.address,reg) 133 | if (self.debug): 134 | print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) 135 | return result 136 | except IOError, err: 137 | return self.errMsg() 138 | 139 | if __name__ == '__main__': 140 | try: 141 | bus = Adafruit_I2C(address=0) 142 | print "Default I2C bus is accessible" 143 | except: 144 | print "Error accessing default I2C bus" 145 | -------------------------------------------------------------------------------- /pi2go/extra_tools/Adafruit_I2C.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/Adafruit_I2C.pyc -------------------------------------------------------------------------------- /pi2go/extra_tools/Adafruit_PWM_Servo_Driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # This is most likely for the full Pi2Go however I shall include it for the sake 3 | # of completeness. It also may come in handy for those using the servo driver 4 | # in another part of their project or on the full Pi2Go. You're welcome ;-) 5 | 6 | import time 7 | import math 8 | from Adafruit_I2C import Adafruit_I2C 9 | 10 | # ============================================================================ 11 | # Adafruit PCA9685 16-Channel PWM Servo Driver 12 | # ============================================================================ 13 | 14 | class PWM : 15 | i2c = None 16 | 17 | # Registers/etc. 18 | __SUBADR1 = 0x02 19 | __SUBADR2 = 0x03 20 | __SUBADR3 = 0x04 21 | __MODE1 = 0x00 22 | __PRESCALE = 0xFE 23 | __LED0_ON_L = 0x06 24 | __LED0_ON_H = 0x07 25 | __LED0_OFF_L = 0x08 26 | __LED0_OFF_H = 0x09 27 | __ALLLED_ON_L = 0xFA 28 | __ALLLED_ON_H = 0xFB 29 | __ALLLED_OFF_L = 0xFC 30 | __ALLLED_OFF_H = 0xFD 31 | 32 | def __init__(self, address=0x40, debug=False): 33 | self.i2c = Adafruit_I2C(address) 34 | # print self.i2c # Matthew Timmons-Brown wanted to stop it from pestering the user with an 'instance' message 35 | self.address = address 36 | self.debug = debug 37 | if (self.debug): 38 | print "Reseting PCA9685" 39 | self.i2c.write8(self.__MODE1, 0x00) 40 | 41 | def setPWMFreq(self, freq): 42 | "Sets the PWM frequency" 43 | prescaleval = 25000000.0 # 25MHz 44 | prescaleval /= 4096.0 # 12-bit 45 | prescaleval /= float(freq) 46 | prescaleval -= 1.0 47 | if (self.debug): 48 | print "Setting PWM frequency to %d Hz" % freq 49 | print "Estimated pre-scale: %d" % prescaleval 50 | prescale = math.floor(prescaleval + 0.5) 51 | if (self.debug): 52 | print "Final pre-scale: %d" % prescale 53 | 54 | oldmode = self.i2c.readU8(self.__MODE1); 55 | newmode = (oldmode & 0x7F) | 0x10 # sleep 56 | self.i2c.write8(self.__MODE1, newmode) # go to sleep 57 | self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) 58 | self.i2c.write8(self.__MODE1, oldmode) 59 | time.sleep(0.005) 60 | self.i2c.write8(self.__MODE1, oldmode | 0x80) 61 | 62 | def setPWM(self, channel, on, off): 63 | "Sets a single PWM channel" 64 | self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF) 65 | self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8) 66 | self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF) 67 | self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8) 68 | -------------------------------------------------------------------------------- /pi2go/extra_tools/Adafruit_PWM_Servo_Driver.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/Adafruit_PWM_Servo_Driver.pyc -------------------------------------------------------------------------------- /pi2go/extra_tools/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /pi2go/extra_tools/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/__init__.pyc -------------------------------------------------------------------------------- /pi2go/extra_tools/info.txt: -------------------------------------------------------------------------------- 1 | This directory stores all of the 'Extra Tools' needed to make the Pi2Go library 2 | function properly. For the Pi2Go-lite most of this code is not needed but I have 3 | included it for the sake of completeness. Also it will come in handy for anyone 4 | using the full version of the Pi2Go. 5 | 6 | Happy Coding! 7 | 8 | -------------------------------------------------------------------------------- /pi2go/extra_tools/line_following/sample_track.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/line_following/sample_track.pdf -------------------------------------------------------------------------------- /pi2go/extra_tools/line_following/track_generator.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/line_following/track_generator.pdf -------------------------------------------------------------------------------- /pi2go/extra_tools/servod.xxx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/servod.xxx -------------------------------------------------------------------------------- /pi2go/extra_tools/sgh_PCF8591P.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/extra_tools/sgh_PCF8591P.pyc -------------------------------------------------------------------------------- /pi2go/line_follower.py: -------------------------------------------------------------------------------- 1 | # Pi2Go 'follower sketch' - for the third episode of my robot tutorial series 2 | # This program is also fairly simple - it utilises the Line IRs 3 | # on the Pi2Go in order to sense obstacles and avoid them 4 | # Created by Matthew Timmons-Brown and Simon Beal 5 | 6 | import pi2go, time 7 | 8 | pi2go.init() 9 | 10 | # Here we set the speed to 60 out of 100 - feel free to change! 11 | speed = 60 12 | 13 | try: 14 | while True: 15 | # Defining the sensors on the bottom of the Pi2Go 16 | left = pi2go.irLeftLine() 17 | right = pi2go.irRightLine() 18 | if left == right: # If both sensors are the same (either on or off): 19 | # Forward 20 | pi2go.forward(speed) 21 | elif left == True: # If the left sensor is on 22 | # Left 23 | pi2go.spinRight(speed) 24 | elif right == True: #If the right sensor is on 25 | # Right 26 | pi2go.spinLeft(speed) 27 | 28 | finally: # Even if there was an error, cleanup 29 | pi2go.cleanup() 30 | -------------------------------------------------------------------------------- /pi2go/motor.py: -------------------------------------------------------------------------------- 1 | # Pi2Go basic motor sketch - for the first episode of my robot tutorial series. 2 | # In truth this program is very simple - the parts where it captures key presses is the daunting bit. 3 | # Try to work through it slowly and you'll soon understand! 4 | 5 | # Use the arrow keys to control the direction of the Pi2Go and use the 'greater than' and 'less than' 6 | # keys to edit the speed! 7 | 8 | import pi2go, time 9 | 10 | # Reading a button press from your keyboard, don't worry about this too much! 11 | import sys 12 | import tty 13 | import termios 14 | 15 | UP = 0 16 | DOWN = 1 17 | RIGHT = 2 18 | LEFT = 3 19 | 20 | def readchar(): 21 | fd = sys.stdin.fileno() 22 | old_settings = termios.tcgetattr(fd) 23 | try: 24 | tty.setraw(sys.stdin.fileno()) 25 | ch = sys.stdin.read(1) 26 | finally: 27 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 28 | if ch == '0x03': 29 | raise KeyboardInterrupt 30 | return ch 31 | 32 | def readkey(getchar_fn=None): 33 | getchar = getchar_fn or readchar 34 | c1 = getchar() 35 | if ord(c1) != 0x1b: 36 | return c1 37 | c2 = getchar() 38 | if ord(c2) != 0x5b: 39 | return c1 40 | c3 = getchar() 41 | return ord(c3) - 65 # 0=Up, 1=Down, 2=Right, 3=Left arrows 42 | 43 | # End of the functions that read your keyboard 44 | 45 | speed = 30 46 | 47 | pi2go.init() 48 | 49 | # Main body of code - this detects your key press and changes direction depending on it 50 | try: 51 | while True: 52 | keyp = readkey() 53 | if keyp == 'w' or keyp == UP: 54 | pi2go.forward(speed) 55 | print 'Forward', speed 56 | elif keyp == 's' or keyp == DOWN: 57 | pi2go.reverse(speed) 58 | print 'Backward', speed 59 | elif keyp == 'd' or keyp == RIGHT: 60 | pi2go.spinRight(speed) 61 | print 'Spin Right', speed 62 | elif keyp == 'a' or keyp == LEFT: 63 | pi2go.spinLeft(speed) 64 | print 'Spin Left', speed 65 | 66 | elif keyp == '.' or keyp == '>': 67 | speed = min(100, speed+10) 68 | print 'Speed+', speed 69 | elif keyp == ',' or keyp == '<': 70 | speed = max (0, speed-10) 71 | print 'Speed-', speed 72 | 73 | elif keyp == ' ': 74 | pi2go.stop() 75 | print 'Stop' 76 | elif ord(keyp) == 3: 77 | break 78 | 79 | # When you want to exit - press ctrl+c and it will generate a keyboard interrupt - this is handled nicely here! 80 | except KeyboardInterrupt: 81 | pi2go.cleanup() 82 | -------------------------------------------------------------------------------- /pi2go/pi2go.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # 3 | # THIS IS THE PI2GO LIBRARY! GREETINGS HUMAN! 4 | # 5 | # Python Module to externalise all Pi2Go specific hardware 6 | # 7 | # Created by Gareth Davies and Zachary Igielman, May 2014 8 | # Updated June 2014 to include Pi2Go-Lite within same framework 9 | # Copyright 4tronix 10 | # 11 | # This code is in the public domain and may be freely copied and used 12 | # No warranty is provided or implied 13 | # 14 | # Utilised by Matthew Timmons-Brown for officially recognised tutorials - significant modification since original script 15 | #====================================================================== 16 | #====================================================================== 17 | # General Functions 18 | # (Both versions) 19 | # 20 | # init(). Initialises GPIO pins, switches motors and LEDs Off, etc 21 | # cleanup(). Sets all motors and LEDs off and sets GPIO to standard values 22 | # version(). Returns 1 for Full Pi2Go, and 2 for Pi2Go-Lite. Invalid until after init() has been called 23 | #====================================================================== 24 | #====================================================================== 25 | # Motor Functions 26 | # (Both Versions) 27 | # 28 | # stop(): Stops both motors 29 | # forward(speed): Sets both motors to move forward at speed. 0 <= speed <= 100 30 | # reverse(speed): Sets both motors to reverse at speed. 0 <= speed <= 100 31 | # spinLeft(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 32 | # spinRight(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 33 | # turnForward(leftSpeed, rightSpeed): Moves forwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 34 | # turnreverse(leftSpeed, rightSpeed): Moves backwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 35 | # go(leftSpeed, rightSpeed): controls motors in both directions independently using different positive/negative speeds. -100<= leftSpeed,rightSpeed <= 100 36 | # go(speed): controls motors in both directions together with positive/negative speed parameter. -100<= speed <= 100 37 | #====================================================================== 38 | #====================================================================== 39 | # RGB LED Functions 40 | # (Full Pi2Go only) 41 | # 42 | # setLED(LED, Red, Green, Blue): Sets the LED specified to required RGB value. 0 >= LED <= 4; 0 <= R,G,B <= 4095 43 | # setAllLEDs(Red, Green, Blue): Sets all LEDs to required RGB. 0 <= R,G,B <= 4095 44 | #====================================================================== 45 | #====================================================================== 46 | # WHITE LED Functions 47 | # (Pi2Go-Lite only) 48 | # 49 | # LsetLED(LED, value): Sets the LED specified to OFF == 0 or ON >= 1 50 | # LsetAllLEDs(value): Sets both LEDs to OFF == 0 or ON >= 1 51 | #====================================================================== 52 | #====================================================================== 53 | # IR Sensor Functions 54 | # (Both Versions) 55 | # 56 | # irLeft(): Returns state of Left IR Obstacle sensor 57 | # irRight(): Returns state of Right IR Obstacle sensor 58 | # irCentre(): Returns state of Centre IR Obstacle sensor (Full Pi2Go Only) 59 | # irAll(): Returns true if any of the Obstacle sensors are triggered 60 | # irLeftLine(): Returns state of Left IR Line sensor 61 | # irRightLine(): Returns state of Right IR Line sensor 62 | #====================================================================== 63 | #====================================================================== 64 | # UltraSonic Functions 65 | # (Both Versions) 66 | # 67 | # getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object 68 | #====================================================================== 69 | #====================================================================== 70 | # Light Sensor Functions 71 | # (Full Pi2Go only) 72 | # 73 | # getLight(Sensor). Returns the value 0..1023 for the selected sensor, 0 <= Sensor <= 3 74 | # getLightFL(). Returns the value 0..1023 for Front-Left light sensor 75 | # getLightFR(). Returns the value 0..1023 for Front-Right light sensor 76 | # getLightBL(). Returns the value 0..1023 for Back-Left light sensor 77 | # getLightBR(). Returns the value 0..1023 for Back-Right light sensor 78 | #====================================================================== 79 | #====================================================================== 80 | # Servo Functions 81 | # 82 | # startServos(). Initialises the servo background process 83 | # stop Servos(). terminates the servo background process 84 | # setServo(Servo, Degrees). Sets the servo to position in degrees -90 to +90 85 | #====================================================================== 86 | #====================================================================== 87 | # Switch Functions 88 | # 89 | # getSwitch(). Returns the value of the tact switch: True==pressed 90 | #====================================================================== 91 | # Import all necessary libraries 92 | import RPi.GPIO as GPIO, sys, threading, time, os 93 | from extra_tools.Adafruit_PWM_Servo_Driver import PWM 94 | from extra_tools.sgh_PCF8591P import sgh_PCF8591P 95 | 96 | GPIO.setwarnings(False) # Pah, who needs runtime errors nowadays?! MTB edit 97 | 98 | # Define Type of Pi2Go 99 | PGNone = 0 100 | PGFull = 1 101 | PGLite = 2 102 | PGType = PGNone # Set to None until we find out which during init() 103 | 104 | # Pins 24, 26 Left Motor 105 | # Pins 19, 21 Right Motor 106 | L1 = 26 107 | L2 = 24 108 | R1 = 19 109 | R2 = 21 110 | 111 | # Define obstacle sensors and line sensors 112 | irFL = 7 113 | irFR = 11 114 | irMID = 15 # this sensor not available on Lite version 115 | lineRight = 13 116 | lineLeft = 12 117 | 118 | # Define Colour IDs for the RGB LEDs (Pi2Go full only) 119 | Blue = 0 120 | Green = 1 121 | Red = 2 122 | pwmMax = 4095 # maximum PWM value 123 | 124 | # Define GPIO pins for Front/rear LEDs on Pi2Go-Lite 125 | frontLED = 15 126 | rearLED = 16 127 | 128 | # Define Sonar Pin (same pin for both Ping and Echo 129 | sonar = 8 130 | 131 | # Define pins for switch (different on each version) 132 | switch = 16 133 | Lswitch = 23 134 | 135 | # Define if servo background process is active 136 | ServosActive = False 137 | 138 | #====================================================================== 139 | # General Functions 140 | # 141 | # init(). Initialises GPIO pins, switches motors and LEDs Off, etc 142 | def init(): 143 | global p, q, a, b, pwm, pcfADC, PGType 144 | PGType = PGFull 145 | # Initialise the PCA9685 PWM device using the default address 146 | try: 147 | pwm = PWM(0x40, debug = False) 148 | pwm.setPWMFreq(60) # Set frequency to 60 Hz 149 | except: 150 | PGType = PGLite # No PCA9685 so set to Pi2Go-Lite 151 | 152 | #use physical pin numbering 153 | GPIO.setmode(GPIO.BOARD) 154 | 155 | #set up digital line detectors as inputs 156 | GPIO.setup(lineRight, GPIO.IN) # Right line sensor 157 | GPIO.setup(lineLeft, GPIO.IN) # Left line sensor 158 | 159 | #Set up IR obstacle sensors as inputs 160 | GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor 161 | GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor 162 | GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor 163 | 164 | #use pwm on inputs so motors don't go too fast 165 | GPIO.setup(L1, GPIO.OUT) 166 | p = GPIO.PWM(L1, 20) 167 | p.start(0) 168 | 169 | GPIO.setup(L2, GPIO.OUT) 170 | q = GPIO.PWM(L2, 20) 171 | q.start(0) 172 | 173 | GPIO.setup(R1, GPIO.OUT) 174 | a = GPIO.PWM(R1, 20) 175 | a.start(0) 176 | 177 | GPIO.setup(R2, GPIO.OUT) 178 | b = GPIO.PWM(R2, 20) 179 | b.start(0) 180 | 181 | # Initalise the ADC 182 | pcfADC = None # ADC object 183 | try: 184 | pcfADC = sgh_PCF8591P(1) #i2c, 0x48) 185 | except: 186 | PGType = PGLite 187 | 188 | # initialise servos (Pi2Go-Lite only) Matt TB - Servo activation not necessary on init 189 | #if PGType == PGLite: 190 | # startServos() 191 | 192 | #set up Pi2Go-Lite White LEDs as outputs 193 | GPIO.setup(frontLED, GPIO.OUT) 194 | GPIO.setup(rearLED, GPIO.OUT) 195 | 196 | #set switch as input with pullup 197 | if PGType == PGLite: 198 | GPIO.setup(Lswitch, GPIO.IN, pull_up_down=GPIO.PUD_UP) 199 | else: 200 | GPIO.setup(switch, GPIO.IN, pull_up_down=GPIO.PUD_UP) 201 | 202 | 203 | # cleanup(). Sets all motors and LEDs off and sets GPIO to standard values 204 | def cleanup(): 205 | stop() 206 | setAllLEDs(0, 0, 0) 207 | stopServod() 208 | time.sleep(1) 209 | GPIO.cleanup() 210 | 211 | 212 | # version(). Returns 1 for Full Pi2Go, and 2 for Pi2Go-Lite. Invalid until after init() has been called 213 | def version(): 214 | return PGType 215 | 216 | # End of General Functions 217 | #====================================================================== 218 | 219 | 220 | #====================================================================== 221 | # Motor Functions 222 | # (both versions) 223 | # 224 | # stop(): Stops both motors 225 | def stop(): 226 | p.ChangeDutyCycle(0) 227 | q.ChangeDutyCycle(0) 228 | a.ChangeDutyCycle(0) 229 | b.ChangeDutyCycle(0) 230 | 231 | # forward(speed): Sets both motors to move forward at speed. 0 <= speed <= 100 232 | def forward(speed): 233 | p.ChangeDutyCycle(speed) 234 | q.ChangeDutyCycle(0) 235 | a.ChangeDutyCycle(speed) 236 | b.ChangeDutyCycle(0) 237 | p.ChangeFrequency(speed + 5) 238 | a.ChangeFrequency(speed + 5) 239 | 240 | # reverse(speed): Sets both motors to reverse at speed. 0 <= speed <= 100 241 | def reverse(speed): 242 | p.ChangeDutyCycle(0) 243 | q.ChangeDutyCycle(speed) 244 | a.ChangeDutyCycle(0) 245 | b.ChangeDutyCycle(speed) 246 | q.ChangeFrequency(speed + 5) 247 | b.ChangeFrequency(speed + 5) 248 | 249 | # spinLeft(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 250 | def spinLeft(speed): 251 | p.ChangeDutyCycle(0) 252 | q.ChangeDutyCycle(speed) 253 | a.ChangeDutyCycle(speed) 254 | b.ChangeDutyCycle(0) 255 | q.ChangeFrequency(speed + 5) 256 | a.ChangeFrequency(speed + 5) 257 | 258 | # spinRight(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 259 | def spinRight(speed): 260 | p.ChangeDutyCycle(speed) 261 | q.ChangeDutyCycle(0) 262 | a.ChangeDutyCycle(0) 263 | b.ChangeDutyCycle(speed) 264 | p.ChangeFrequency(speed + 5) 265 | b.ChangeFrequency(speed + 5) 266 | 267 | # turnForward(leftSpeed, rightSpeed): Moves forwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 268 | def turnForward(leftSpeed, rightSpeed): 269 | p.ChangeDutyCycle(leftSpeed) 270 | q.ChangeDutyCycle(0) 271 | a.ChangeDutyCycle(rightSpeed) 272 | b.ChangeDutyCycle(0) 273 | p.ChangeFrequency(leftSpeed + 5) 274 | a.ChangeFrequency(rightSpeed + 5) 275 | 276 | # turnReverse(leftSpeed, rightSpeed): Moves backwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 277 | def turnReverse(leftSpeed, rightSpeed): 278 | p.ChangeDutyCycle(0) 279 | q.ChangeDutyCycle(leftSpeed) 280 | a.ChangeDutyCycle(0) 281 | b.ChangeDutyCycle(rightSpeed) 282 | q.ChangeFrequency(leftSpeed + 5) 283 | b.ChangeFrequency(rightSpeed + 5) 284 | 285 | # go(leftSpeed, rightSpeed): controls motors in both directions independently using different positive/negative speeds. -100<= leftSpeed,rightSpeed <= 100 286 | def go(leftSpeed, rightSpeed): 287 | if leftSpeed<0: 288 | p.ChangeDutyCycle(0) 289 | q.ChangeDutyCycle(abs(leftSpeed)) 290 | q.ChangeFrequency(abs(leftSpeed) + 5) 291 | else: 292 | q.ChangeDutyCycle(0) 293 | p.ChangeDutyCycle(leftSpeed) 294 | p.ChangeFrequency(leftSpeed + 5) 295 | if rightSpeed<0: 296 | a.ChangeDutyCycle(0) 297 | b.ChangeDutyCycle(abs(rightSpeed)) 298 | p.ChangeFrequency(abs(rightSpeed) + 5) 299 | else: 300 | b.ChangeDutyCycle(0) 301 | a.ChangeDutyCycle(rightSpeed) 302 | p.ChangeFrequency(rightSpeed + 5) 303 | 304 | # go(speed): controls motors in both directions together with positive/negative speed parameter. -100<= speed <= 100 305 | def goBoth(speed): 306 | if speed<0: 307 | reverse(abs(speed)) 308 | else: 309 | forward(speed) 310 | 311 | # End of Motor Functions 312 | #====================================================================== 313 | 314 | 315 | #====================================================================== 316 | # RGB LED Functions 317 | # (Full version only) 318 | # 319 | # setLED(LED, Red, Green, Blue): Sets the LED specified to required RGB value. 0 >= LED <= 3; 0 <= R,G,B <= 4095 320 | def setLED(LED, red, green, blue): 321 | if PGType == PGFull: 322 | pwm.setPWM(LED * 3 + Red, 0, red) 323 | pwm.setPWM(LED * 3 + Green, 0, green) 324 | pwm.setPWM(LED * 3 + Blue, 0, blue) 325 | 326 | # setAllLEDs(Red, Green, Blue): Sets all LEDs to required RGB. 0 <= R,G,B <= 4095 327 | def setAllLEDs (red, green, blue): 328 | for i in range(4): 329 | setLED(i, red, green, blue) 330 | 331 | # End of RGB LED Functions 332 | #====================================================================== 333 | 334 | 335 | #====================================================================== 336 | # White LED Functions 337 | # (Pi2Go-Lite only) 338 | # 339 | # LsetLED(LED, value): Sets the LED specified to OFF == 0 or ON == 1 340 | # TODO: take value from 0 to 100 and use as percentage PWM value 341 | def LsetLED (LED, value): 342 | if PGType == PGLite: 343 | if value == 0: 344 | value = 1 345 | else: 346 | value = 0 347 | if LED == 0: 348 | GPIO.output (frontLED, value) 349 | else: 350 | GPIO.output (rearLED, value) 351 | 352 | # LsetAllLEDs(value): Sets both LEDs to OFF == 0 or ON == 1 353 | 354 | # End of White LED Functions 355 | #====================================================================== 356 | 357 | 358 | #====================================================================== 359 | # IR Sensor Functions 360 | # 361 | # irLeft(): Returns state of Left IR Obstacle sensor 362 | def irLeft(): 363 | if GPIO.input(irFL)==0: 364 | return True 365 | else: 366 | return False 367 | 368 | # irRight(): Returns state of Right IR Obstacle sensor 369 | def irRight(): 370 | if GPIO.input(irFR)==0: 371 | return True 372 | else: 373 | return False 374 | 375 | # irCentre(): Returns state of Centre IR Obstacle sensor 376 | # (Not available on Pi2Go-Lite) 377 | def irCentre(): 378 | if PGType != PGFull: 379 | return False 380 | if GPIO.input(irMID)==0: 381 | return True 382 | else: 383 | return False 384 | 385 | # irAll(): Returns true if any of the Obstacle sensors are triggered 386 | def irAll(): 387 | if GPIO.input(irFL)==0 or GPIO.input(irFR)==0 or (PGType==PGFull and GPIO.input(irMID)==0): 388 | return True 389 | else: 390 | return False 391 | 392 | # irLeftLine(): Returns state of Left IR Line sensor 393 | def irLeftLine(): 394 | if GPIO.input(lineLeft)==0: 395 | return True 396 | else: 397 | return False 398 | 399 | # irRightLine(): Returns state of Right IR Line sensor 400 | def irRightLine(): 401 | if GPIO.input(lineRight)==0: 402 | return True 403 | else: 404 | return False 405 | 406 | # End of IR Sensor Functions 407 | #====================================================================== 408 | 409 | 410 | #====================================================================== 411 | # UltraSonic Functions 412 | # 413 | # getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object 414 | # (Both versions) 415 | # 416 | def getDistance(): 417 | GPIO.setup(sonar, GPIO.OUT) 418 | # Send 10us pulse to trigger 419 | GPIO.output(sonar, True) 420 | time.sleep(0.00001) 421 | GPIO.output(sonar, False) 422 | start = time.time() 423 | count=time.time() 424 | GPIO.setup(sonar,GPIO.IN) 425 | while GPIO.input(sonar)==0 and time.time()-count<0.1: 426 | start = time.time() 427 | count=time.time() 428 | stop=count 429 | while GPIO.input(sonar)==1 and time.time()-count<0.1: 430 | stop = time.time() 431 | # Calculate pulse length 432 | elapsed = stop-start 433 | # Distance pulse travelled in that time is time 434 | # multiplied by the speed of sound 34000(cm/s) divided by 2 435 | distance = elapsed * 17000 436 | return distance 437 | 438 | # End of UltraSonic Functions 439 | #====================================================================== 440 | 441 | 442 | #====================================================================== 443 | # Light Sensor Functions 444 | # (Full Pi2Go Only) 445 | # 446 | # getLight(sensor). Returns the value 0..1023 for the selected sensor, 0 <= Sensor <= 3 447 | def getLight(sensor): 448 | if PGType != PGFull: 449 | return False 450 | value = pcfADC.readADC(sensor) 451 | return value 452 | 453 | # getLightFL(). Returns the value 0..1023 for Front-Left light sensor 454 | def getLightFL(): 455 | if PGType != PGFull: 456 | return False 457 | value = pcfADC.readADC(0) 458 | return value 459 | 460 | # getLightFR(). Returns the value 0..1023 for Front-Right light sensor 461 | def getLightFR(sensor): 462 | if PGType != PGFull: 463 | return False 464 | value = pcfADC.readADC(1) 465 | return value 466 | 467 | # getLightBL(). Returns the value 0..1023 for Back-Left light sensor 468 | def getLightBL(sensor): 469 | if PGType != PGFull: 470 | return False 471 | value = pcfADC.readADC(2) 472 | return value 473 | 474 | # getLightBR(). Returns the value 0..1023 for Back-Right light sensor 475 | def getLightBR(sensor): 476 | if PGType != PGFull: 477 | return False 478 | value = pcfADC.readADC(3) 479 | return value 480 | 481 | # End of Light Sensor Functions 482 | #====================================================================== 483 | 484 | 485 | #====================================================================== 486 | # Switch Functions 487 | # 488 | # getSwitch(). Returns the value of the tact switch: True==pressed 489 | def getSwitch(): 490 | if PGType == 1: 491 | val = GPIO.input(switch) 492 | else: 493 | val = GPIO.input(Lswitch) 494 | return (val == 0) 495 | # 496 | # End of switch functions 497 | #====================================================================== 498 | 499 | 500 | 501 | #====================================================================== 502 | # Servo Functions 503 | # Pi2Go-Lite uses ServoD to control servos 504 | # Pi2Go Full uses the PCA9685 hardware controller 505 | 506 | def setServo(Servo, Degrees): 507 | #print "ServosActive:", ServosActive 508 | if ServosActive == False: 509 | startServos() 510 | pinServod (Servo, Degrees) # for now, simply pass on the input values 511 | 512 | def stopServos(): 513 | stopServod() 514 | 515 | def startServos(): 516 | startServod() 517 | 518 | def startServod(): 519 | global ServosActive 520 | #print "Starting servod. ServosActove:", ServosActive 521 | SCRIPTPATH = os.path.split(os.path.realpath(__file__))[0] 522 | #os.system("sudo pkill -f servod") 523 | os.system(SCRIPTPATH +'/servod --idle-timeout=20000 --p1pins="18,22"') 524 | #print (SCRIPTPATH +'/servod --idle-timeout=20000 --p1pins="18,22"') 525 | ServosActive = True 526 | 527 | def pinServod(pin, degrees): 528 | #print pin, degrees 529 | #print ("echo " + str(pin) + "=" + str(50+ ((90 - degrees) * 200 / 180)) + " > /dev/servoblaster") 530 | os.system("echo " + str(pin) + "=" + str(50+ ((90 - degrees) * 200 / 180)) + " > /dev/servoblaster") 531 | 532 | def stopServod(): 533 | global ServosActive 534 | os.system("sudo pkill -f servod") 535 | ServosActive = False 536 | 537 | 538 | 539 | -------------------------------------------------------------------------------- /pi2go/pi2go.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/the-raspberry-pi-guy/robot/73d023f36f23e14722420923d4342ebf63878ccb/pi2go/pi2go.pyc --------------------------------------------------------------------------------