├── .gitattributes ├── .gitignore ├── Behaviors └── LineBehavior.py ├── Constants.py ├── Examples ├── irTest.py ├── motorTest.py ├── motorTest2.py ├── motorTestRaw.py ├── servo.py ├── servoTest.py ├── sonarTest.py └── ssonarTest.py ├── HardwareLibs ├── Camera.py ├── RoboHat.py ├── Rover.py ├── Wheel.py └── servod ├── Main.py ├── Utils.py ├── VisionUtils.py └── robohat.pyc /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear in the root of a volume 35 | .DocumentRevisions-V100 36 | .fseventsd 37 | .Spotlight-V100 38 | .TemporaryItems 39 | .Trashes 40 | .VolumeIcon.icns 41 | 42 | # Directories potentially created on remote AFP share 43 | .AppleDB 44 | .AppleDesktop 45 | Network Trash Folder 46 | Temporary Items 47 | .apdisk 48 | -------------------------------------------------------------------------------- /Behaviors/LineBehavior.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import VisionUtils 4 | from Utils import lineAngle, clamp 5 | from collections import namedtuple 6 | from time import time, sleep 7 | 8 | 9 | class Line: 10 | def __init__(self, p1, p2): 11 | # Point: [x, y] or (x, y) 12 | self.p1 = tuple(p1) 13 | self.p2 = tuple(p2) 14 | 15 | 16 | # Angle is 130 if the line points approximately from the bottom right to top left 17 | # Angle is 60 if the line points approximately from the bottom left to the top right 18 | self.angle = 180 - lineAngle(self.p1, self.p2) 19 | 20 | def __str__(self): 21 | return "Angle: " + str(self.angle) + " P1: " + str(self.p1) + " P2: " + str(self.p2) 22 | 23 | def __iter__(self): 24 | for point in [self.p1, self.p2]: 25 | yield point 26 | 27 | 28 | class Mapper: 29 | """ 30 | This class keeps track of new lines and identifies if the newly recognized line is consistent with the last 31 | few frames or if it's a mistake from bad line. 32 | 33 | It also performs the important task of keeping track of the map, where junctions are, and attempting to localize 34 | the robot within what it knows of the lines so far. 35 | """ 36 | 37 | def __init__(self): 38 | self.history = [] # A list of line lists [[L1, L2], [L1, L2, L3] from previous frames 39 | self.currentLine = None 40 | 41 | def addLineFrame(self, lines): 42 | """ 43 | :param lines: A list of [Line, Line] that the camera thinks it has found 44 | :return: 45 | """ 46 | 47 | if len(lines) == 0: 48 | self.currentLine = None 49 | return 50 | 51 | 52 | self.history.append(lines) 53 | 54 | # Find the highest 'y' of any line 55 | highestY = (0, 0) # [0] is the line index, [1] is the highest y value 56 | for index, line in enumerate(self.history[-1]): 57 | if line.p1[1] > highestY[1]: 58 | highestY = (index, line.p1[1]) 59 | elif line.p2[1] > highestY[1]: 60 | highestY = (index, line.p2[1]) 61 | self.currentLine = self.history[-1][highestY[0]] 62 | 63 | def getCurrentLine(self): 64 | return self.currentLine 65 | 66 | 67 | class FollowLine: 68 | 69 | def __init__(self, parent): 70 | self.rover = parent 71 | self.map = Mapper() 72 | self.targetSpeed = 300 73 | 74 | self.framesSinceLine = 0 # How many frames since the line was seen 75 | 76 | 77 | def update(self): 78 | lowRed = [150, 75, 75] 79 | highRed = [30, 255, 255] 80 | 81 | lines = self.__findLines(lowRed, highRed) 82 | self.map.addLineFrame(lines) 83 | 84 | line = self.map.getCurrentLine() # Gets direction of the currently followed line 85 | 86 | if line is None: 87 | self.framesSinceLine += 1 88 | if self.framesSinceLine > 50: 89 | self.rover.LWheel.setSpeed(0) 90 | self.rover.RWheel.setSpeed(0) 91 | return 92 | 93 | self.framesSinceLine = 0 94 | 95 | # Pick the point to move towards 96 | highestPoint = sorted(line, key=lambda l: l[1])[0] 97 | print(line, highestPoint) 98 | # self.moveTowards(highestPoint) 99 | 100 | 101 | # Robot Control Functions 102 | def moveTowards(self, point): 103 | """ 104 | Move towards a Line 105 | :param point: [x, y] 106 | :return: 107 | """ 108 | 109 | lWheel = self.rover.LWheel 110 | rWheel = self.rover.RWheel 111 | 112 | # Get the highest point of the line 113 | horzMiddle = self.rover.camera.resolution[0] / 2 114 | vertMax = self.rover.camera.resolution[1] 115 | xMag = (point[0] - horzMiddle) / horzMiddle # -1 to 1, where -1 is left and 1 is right 116 | yMag = (vertMax - point[1]) / vertMax # 0 to 1, where 1 is toop and 0 is bottom 117 | 118 | 119 | turnSpeed = (self.targetSpeed * .4) * xMag # * yMag # clamp(self.targetSpeed * yMag, self.targetSpeed*.5, self.targetSpeed) 120 | lSpeed = self.targetSpeed*.6 + turnSpeed # Where -1 xmag will lower left turning speed 121 | rSpeed = self.targetSpeed*.6 - turnSpeed # Where -1 xmag will raise the right turning speed 122 | lSpeed = clamp(int(lSpeed), -self.targetSpeed * .1, self.targetSpeed) 123 | rSpeed = clamp(int(rSpeed), -self.targetSpeed * .1, self.targetSpeed) 124 | 125 | # Make sure stuff doesn't get too fast! 126 | if lSpeed + rSpeed >= self.targetSpeed * 2: 127 | amountOver = lSpeed + rSpeed - self.targetSpeed * 2 128 | lSpeed -= amountOver / 2 129 | rSpeed -= amountOver / 2 130 | 131 | lWheel.setSpeed(lSpeed) 132 | rWheel.setSpeed(rSpeed) 133 | 134 | print("Left: ", lSpeed, "\tRight: ", rSpeed, "\txMag", xMag, "\tyMag", yMag) 135 | 136 | # if point[0] < horzMiddle: 137 | # print("Left") 138 | # lWheel.setSpeed(0) 139 | # rWheel.setSpeed(self.targetSpeed) 140 | # return 141 | # 142 | # if point[0] > horzMiddle: 143 | # print("Right") 144 | # lWheel.setSpeed(self.targetSpeed) 145 | # rWheel.setSpeed(0) 146 | # return 147 | 148 | """ 149 | lowerThresh = 89 150 | upperThresh = 91 151 | # Straight 152 | if lowerThresh <= line.angle <= upperThresh: 153 | print("Straight") 154 | lWheel.setSpeed(self.targetSpeed) 155 | rWheel.setSpeed(self.targetSpeed) 156 | return 157 | 158 | # Left 159 | if line.angle < lowerThresh: 160 | print("Left") 161 | lWheel.setSpeed(self.targetSpeed*.5) 162 | rWheel.setSpeed(self.targetSpeed) 163 | return 164 | 165 | # Right 166 | if line.angle > upperThresh: 167 | print("Right") 168 | lWheel.setSpeed(self.targetSpeed) 169 | rWheel.setSpeed(self.targetSpeed*.5) 170 | return 171 | """ 172 | 173 | # Line Identification Functions 174 | def __findLines(self, hueLow, hueHigh): 175 | img = self.rover.camera.read() 176 | 177 | rImg = VisionUtils.isolateColor(img, hueLow, hueHigh) 178 | rGray = cv2.cvtColor(rImg, cv2.COLOR_BGR2GRAY) 179 | ret, rThresh = cv2.threshold(rGray, 50, 255, cv2.THRESH_BINARY) 180 | 181 | # Make the image small to reduce line-finding processing times 182 | small = cv2.resize(rThresh, (64, 48), interpolation=cv2.INTER_AREA) 183 | 184 | 185 | 186 | 187 | # lines = cv2.HoughLinesP(edges, 1, np.pi, threshold=25, minLineLength=50, maxLineGap=10) 188 | lines = cv2.HoughLinesP(small, 1, np.pi/200, threshold=25, minLineLength=20, maxLineGap=10) 189 | 190 | 191 | if lines is None: return [] 192 | 193 | cv2.imshow('final', rThresh) 194 | cv2.waitKey(2500) 195 | 196 | # If lines were found, combine them until you have 1 average for each 'direction' of tape in the photo 197 | lines = [line[0] for line in lines] 198 | combinedLines = self.__combineLines(lines) 199 | 200 | 201 | return combinedLines 202 | 203 | def __combineLines(self, unsortedLines): 204 | """ Combines similar lines into one large 'average' line """ 205 | 206 | maxAngle = 45 207 | minLinesForCombo = 5 208 | 209 | def getAngle(line): 210 | # Turn angle from -180:180 to just 0:180 211 | angle = lineAngle(line[:2], line[2:]) 212 | if angle < 0: angle += 180 213 | return angle 214 | 215 | def lineFits(checkLine, combo): 216 | """ Check if the line fits within this group of combos by checking it's angle """ 217 | checkAngle = getAngle(checkLine) 218 | for line in combo: 219 | angle = lineAngle(line[:2], line[2:]) 220 | difference = abs(checkAngle - angle) 221 | 222 | if difference < maxAngle or 180 - difference < maxAngle: 223 | return True 224 | # if difference > maxAngle * 2 or 180 - difference > maxAngle * 2: 225 | # return False 226 | return False 227 | 228 | # Pre-process lines so that lines always point from 0 degrees to 180, and not over 229 | for i, line in enumerate(unsortedLines): 230 | angle = lineAngle(line[:2], line[2:]) 231 | if angle < 0: 232 | line = np.concatenate((line[2:], line[:2])) 233 | unsortedLines[i] = line 234 | 235 | 236 | # Get Line Combos 237 | lineCombos = [] # Format: [[[l1, l2, l3], [l4, l5, l6]], [[line 1...], [line 2...]]] 238 | 239 | while len(unsortedLines) > 0: 240 | checkLine = unsortedLines.pop(0) 241 | 242 | isSorted = False 243 | for i, combo in enumerate(lineCombos): 244 | if lineFits(checkLine, combo): 245 | # Process the line so that the [x1, y1, and x2, y2] are in the same positions as other combos 246 | lineCombos[i].append(checkLine.tolist()) 247 | isSorted = True 248 | break 249 | 250 | if not isSorted: 251 | lineCombos.append([checkLine.tolist()]) 252 | 253 | 254 | # # Limit each combo to minSamples, keeping only the longest lines 255 | # lineCombos = [sorted(combo, key= lambda c: (c[0] - c[2]) ** 2 + (c[1] - c[3]) ** 2, reverse=True) 256 | # for combo in lineCombos] 257 | # lineCombos = [combo[:minLinesForCombo] for combo in lineCombos] 258 | 259 | 260 | # Filter and Average Combo Groups Format: [[L1], [L2], [L3]] 261 | averagedCombos = [] 262 | for combo in lineCombos: 263 | if len(combo) < minLinesForCombo: continue 264 | 265 | avgLine = (np.sum(combo, axis=0) / len(combo)).astype(int) 266 | avgLine *= 10 # Rescale to screen size 267 | averagedCombos.append(Line(avgLine[:2], avgLine[2:])) 268 | 269 | 270 | # # Draw Line Combos and Final Lines 271 | # img = self.rover.camera.read() 272 | # for i, combo in enumerate(lineCombos): 273 | # for x1, y1, x2, y2 in combo: 274 | # x1 *= 10 275 | # y1 *= 10 276 | # x2 *= 10 277 | # y2 *= 10 278 | # 279 | # cv2.line(img, (x1, y1), (x2, y2), (80*i, 80*i, 80*i), 2) 280 | # 281 | # if len(averagedCombos): 282 | # for p1, p2 in averagedCombos: 283 | # x1 = p1[0] 284 | # y1 = p1[1] 285 | # x2 = p2[0] 286 | # y2 = p2[1] 287 | # 288 | # cv2.line(img, (x1, y1), (x2, y2), (80, 80, 80), 8) 289 | # 290 | # cv2.imshow('final', img) 291 | # cv2.waitKey(2500) 292 | 293 | return averagedCombos 294 | 295 | 296 | 297 | 298 | 299 | -------------------------------------------------------------------------------- /Constants.py: -------------------------------------------------------------------------------- 1 | ###### DIMENSIONS 2 | 3 | # mm Distance from the center of one wheel to the center of the opposite wheel 4 | distBetweenWheels = 135.5 5 | 6 | 7 | ###### WHEEL CONSTANTS 8 | 9 | # (MM/tick) Distance the wheel travels per tick of the encoder 10 | mmPerEncoderTick = 4.83308845108 * 2 11 | 12 | 13 | ###### PIN CONSTANTS 14 | cameraPanPin = 1 15 | cameraTiltPin = 0 16 | 17 | leftWheelPinA = 36 18 | leftWheelPinB = 35 19 | rightWheelPinA = 33 20 | rightWheelPinB = 32 21 | 22 | leftEncoderPinA = 13 23 | leftEncoderPinB = 29 24 | rightEncoderPinA = 15 25 | rightEncoderPinB = 16 26 | 27 | 28 | ###### PAN/TILT CONSTANTS 29 | panOffset = 15 # Degree offset -------------------------------------------------------------------------------- /Examples/irTest.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from HardwareLibs import RoboHat 4 | 5 | RoboHat.init() 6 | 7 | 8 | 9 | try: 10 | lastL = RoboHat.irLeft() 11 | lastR = RoboHat.irRight() 12 | lastLineL = RoboHat.irLeftLine() 13 | lastLineR = RoboHat.irRightLine() 14 | print 'Left, Right, LeftLine, RightLine:', lastL, lastR, lastLineL, lastLineR 15 | print 16 | while True: 17 | newL = RoboHat.irLeft() 18 | newR = RoboHat.irRight() 19 | newLineL = RoboHat.irLeftLine() 20 | newLineR = RoboHat.irRightLine() 21 | if (newL != lastL) or (newR != lastR) or (newLineL != lastLineL) or (newLineR != lastLineR): 22 | print 'Left, Right, LeftLine, RightLine:', newL, newR, newLineL, newLineR 23 | print 24 | lastL = newL 25 | lastR = newR 26 | lastLineL = newLineL 27 | lastLineR = newLineR 28 | time.sleep(0.1) 29 | 30 | except KeyboardInterrupt: 31 | print 32 | 33 | finally: 34 | RoboHat.cleanup() 35 | -------------------------------------------------------------------------------- /Examples/motorTest.py: -------------------------------------------------------------------------------- 1 | # initio Motor Test 2 | # Moves: Forward, Reverse, turn Right, turn Left, Stop - then repeat 3 | # Press Ctrl-C to stop 4 | # 5 | # Also demonstrates writing to the LEDs 6 | # 7 | # To check wiring is correct ensure the order of movement as above is correct 8 | # Run using: sudo python motorTest.py 9 | 10 | 11 | import time 12 | 13 | from HardwareLibs import RoboHat 14 | 15 | speed = 80 16 | print "Tests the motors at speed = 80%" 17 | print "Forward, Reverse, Spin Right, Spin Left, Stop, then repeat" 18 | print "Press Ctrl-C to stop" 19 | print 20 | 21 | RoboHat.init() 22 | 23 | # main loop 24 | try: 25 | while True: 26 | RoboHat.forward(speed) 27 | print 'Forward' 28 | time.sleep(3) 29 | RoboHat.reverse(speed) 30 | print 'Reverse' 31 | time.sleep(3) 32 | RoboHat.spinRight(speed) 33 | print 'Spin Right' 34 | time.sleep(3) 35 | RoboHat.spinLeft(speed) 36 | print 'Spin Left' 37 | time.sleep(3) 38 | RoboHat.stop() 39 | print 'Stop' 40 | time.sleep(3) 41 | 42 | except KeyboardInterrupt: 43 | print 44 | 45 | finally: 46 | RoboHat.cleanup() 47 | 48 | -------------------------------------------------------------------------------- /Examples/motorTest2.py: -------------------------------------------------------------------------------- 1 | # Initio Motor Test 2 | # Moves: Forward, Reverse, turn Right, turn Left, Stop - then repeat 3 | # Press Ctrl-C to stop 4 | # 5 | # To check wiring is correct ensure the order of movement as above is correct 6 | # Run using: sudo python motorTest2.py 7 | 8 | 9 | import sys 10 | import tty 11 | import termios 12 | from HardwareLibs import RoboHat 13 | 14 | def readchar(): 15 | fd = sys.stdin.fileno() 16 | old_settings = termios.tcgetattr(fd) 17 | try: 18 | tty.setraw(sys.stdin.fileno()) 19 | ch = sys.stdin.read(1) 20 | finally: 21 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 22 | if ch == '0x03': 23 | raise KeyboardInterrupt 24 | return ch 25 | 26 | def readkey(getchar_fn=None): 27 | getchar = getchar_fn or readchar 28 | c1 = getchar() 29 | if ord(c1) != 0x1b: 30 | return c1 31 | c2 = getchar() 32 | if ord(c2) != 0x5b: 33 | return c1 34 | c3 = getchar() 35 | return chr(0x10 + ord(c3) - 65) # 16=Up, 17=Down, 18=Right, 19=Left arrows 36 | 37 | # End of single character reading 38 | #====================================================================== 39 | 40 | speed = 60 41 | 42 | print "Tests the motors by using the arrow keys to control" 43 | print "Use , or < to slow down" 44 | print "Use . or > to speed up" 45 | print "Speed changes take effect when the next arrow key is pressed" 46 | print "Press Ctrl-C to end" 47 | print 48 | 49 | RoboHat.init() 50 | 51 | # main loop 52 | try: 53 | while True: 54 | keyp = readkey() 55 | if keyp == 'w' or ord(keyp) == 16: 56 | RoboHat.forward(speed) 57 | print 'Forward', speed 58 | elif keyp == 'z' or ord(keyp) == 17: 59 | RoboHat.reverse(speed) 60 | print 'Reverse', speed 61 | elif keyp == 's' or ord(keyp) == 18: 62 | RoboHat.spinRight(speed) 63 | print 'Spin Right', speed 64 | elif keyp == 'a' or ord(keyp) == 19: 65 | RoboHat.spinLeft(speed) 66 | print 'Spin Left', speed 67 | elif keyp == '.' or keyp == '>': 68 | speed = min(100, speed+10) 69 | print 'Speed+', speed 70 | elif keyp == ',' or keyp == '<': 71 | speed = max (0, speed-10) 72 | print 'Speed-', speed 73 | elif keyp == ' ': 74 | RoboHat.stop() 75 | print 'Stop' 76 | elif ord(keyp) == 3: 77 | break 78 | 79 | except KeyboardInterrupt: 80 | print 81 | 82 | finally: 83 | RoboHat.cleanup() 84 | 85 | -------------------------------------------------------------------------------- /Examples/motorTestRaw.py: -------------------------------------------------------------------------------- 1 | # initio Motor Test using RoboHAT 2 | # This uses the GPIO pins in "raw", ie. without using the library 3 | # Moves: Forward, Reverse, turn Right, turn Left, Stop - then repeat 4 | # Press Ctrl-C to stop 5 | # 6 | # Writes directly to motors without using the library 7 | # 8 | # To check wiring is correct ensure the order of movement as above is correct 9 | # Run using: sudo python motorTesta.py 10 | 11 | 12 | import time, RPi.GPIO as gpio 13 | 14 | #Motors 15 | L1 = 36 16 | L2 = 35 17 | R1 = 33 18 | R2 = 32 19 | 20 | gpio.setmode(gpio.BOARD) 21 | gpio.setup(L1, gpio.OUT) 22 | gpio.setup(L2, gpio.OUT) 23 | gpio.setup(R1, gpio.OUT) 24 | gpio.setup(R2, gpio.OUT) 25 | 26 | 27 | def forward(): 28 | gpio.output(L1, 1) 29 | gpio.output(L2, 0) 30 | gpio.output(R1, 1) 31 | gpio.output(R2, 0) 32 | 33 | def reverse(): 34 | gpio.output(L1, 0) 35 | gpio.output(L2, 1) 36 | gpio.output(R1, 0) 37 | gpio.output(R2, 1) 38 | 39 | def spinLeft(): 40 | gpio.output(L1, 0) 41 | gpio.output(L2, 1) 42 | gpio.output(R1, 1) 43 | gpio.output(R2, 0) 44 | 45 | def spinRight(): 46 | gpio.output(L1, 1) 47 | gpio.output(L2, 0) 48 | gpio.output(R1, 0) 49 | gpio.output(R2, 1) 50 | 51 | def stop(): 52 | gpio.output(L1, 0) 53 | gpio.output(L2, 0) 54 | gpio.output(R1, 0) 55 | gpio.output(R2, 0) 56 | 57 | print "Moves: Forward, Reverse, turn Right, turn Left, Stop - then repeat" 58 | print "Press Ctrl-C to stop" 59 | print 60 | 61 | # main loop 62 | try: 63 | while True: 64 | forward() 65 | print 'Forward' 66 | time.sleep(3) 67 | reverse() 68 | print 'Reverse' 69 | time.sleep(3) 70 | spinRight() 71 | print 'Spin Right' 72 | time.sleep(3) 73 | spinLeft() 74 | print 'Spin Left' 75 | time.sleep(3) 76 | stop() 77 | print 'Stop' 78 | time.sleep(3) 79 | 80 | except KeyboardInterrupt: 81 | print 82 | 83 | finally: 84 | gpio.cleanup() 85 | 86 | -------------------------------------------------------------------------------- /Examples/servo.py: -------------------------------------------------------------------------------- 1 | # initio Servo Test using simple PWM 2 | # Do not expect this to be reliable or stable 3 | # Moves the servo left, centre, right and repeats 4 | # Press Ctrl-C to stop 5 | # 6 | # Please use servoTest.py for correct operation 7 | # 8 | # Run using: sudo python servoTest.py 9 | 10 | 11 | import time, RPi.GPIO as gpio 12 | 13 | servo = 22 14 | 15 | gpio.setmode(gpio.BOARD) 16 | gpio.setup(servo, gpio.OUT) 17 | 18 | 19 | p = gpio.PWM(servo, 200) # frequency is 500Hz, so each pulse is 5ms wide 20 | # servos will be fully left at 0.5ms, centred at 1.5ms and fully right at 2.5ms 21 | 22 | left = 50/5 23 | right = 250/5 24 | centre = 150/5 25 | 26 | p.start(centre) # start it at 50% - should be centre of servo 27 | #p.ChangeDutyCycle(100) 28 | 29 | print "Testing servo on pin", servo 30 | print 31 | 32 | # main loop 33 | try: 34 | while True: 35 | p.ChangeDutyCycle(centre) 36 | print 'Centre' 37 | time.sleep(2) 38 | p.ChangeDutyCycle(left) 39 | print 'Left' 40 | time.sleep(2) 41 | p.ChangeDutyCycle(centre) 42 | print 'Centre' 43 | time.sleep(2) 44 | p.ChangeDutyCycle(right) 45 | print 'Right' 46 | time.sleep(2) 47 | 48 | except KeyboardInterrupt: 49 | print 50 | 51 | finally: 52 | gpio.cleanup() 53 | 54 | -------------------------------------------------------------------------------- /Examples/servoTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # servoTest.py 3 | 4 | from HardwareLibs import RoboHat 5 | 6 | # Define pins for Pan/Tilt 7 | pan = 0 8 | tilt = 1 9 | tVal = 0 # 0 degrees is centre 10 | pVal = 0 # 0 degrees is centre 11 | 12 | #====================================================================== 13 | # Reading single character by forcing stdin to raw mode 14 | import sys 15 | import tty 16 | import termios 17 | 18 | def readchar(): 19 | fd = sys.stdin.fileno() 20 | old_settings = termios.tcgetattr(fd) 21 | try: 22 | tty.setraw(sys.stdin.fileno()) 23 | ch = sys.stdin.read(1) 24 | finally: 25 | termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) 26 | if ch == '0x03': 27 | raise KeyboardInterrupt 28 | return ch 29 | 30 | def readkey(getchar_fn=None): 31 | getchar = getchar_fn or readchar 32 | c1 = getchar() 33 | if ord(c1) != 0x1b: 34 | return c1 35 | c2 = getchar() 36 | if ord(c2) != 0x5b: 37 | return c1 38 | c3 = getchar() 39 | return chr(0x10 + ord(c3) - 65) # 16=Up, 17=Down, 18=Right, 19=Left arrows 40 | 41 | # End of single character reading 42 | #====================================================================== 43 | 44 | 45 | RoboHat.init() 46 | #print "Robohat version: ", robohat.version() 47 | 48 | def doServos(): 49 | RoboHat.setServo(pan, pVal) 50 | RoboHat.setServo(tilt, tVal) 51 | 52 | print "Use Arrows or W-Up, Z-Down, A-Left, S-Right Space=Centre, ^C=Exit:" 53 | 54 | try: 55 | while True: 56 | key = readkey() 57 | if key == ' ': 58 | tVal = 0 59 | pVal = 0 60 | doServos() 61 | print "Centre", tVal, pVal 62 | elif key.upper() == 'L': 63 | tVal = -90 64 | pVal = -90 65 | doServos() 66 | print "Left", tVal, pVal 67 | elif key.upper() == 'R': 68 | tVal = 90 69 | pVal = 90 70 | doServos() 71 | print "Right", tVal, pVal 72 | elif key ==' x' or key == '.': 73 | initio.stopServos() 74 | print "Stop" 75 | 76 | elif key == 'w' or ord(key) == 16: 77 | pVal = min(90, pVal+10) 78 | doServos() 79 | print "Up", pVal 80 | 81 | elif key == 'a' or ord(key) == 19: 82 | tVal = max (-90, tVal-10) 83 | doServos() 84 | print "Left", tVal 85 | 86 | elif key == 's' or ord(key) == 18: 87 | tVal = min(90, tVal+10) 88 | doServos() 89 | print "Right", tVal 90 | 91 | elif key == 'z' or ord(key) == 17: 92 | pVal = max(-90, pVal-10) 93 | doServos() 94 | print "Down", pVal 95 | 96 | elif key == 'g': 97 | RoboHat.startServos() 98 | print "Down" 99 | elif ord(key) == 3: 100 | break 101 | 102 | except KeyboardInterrupt: 103 | print 104 | 105 | finally: 106 | RoboHat.cleanup() 107 | -------------------------------------------------------------------------------- /Examples/sonarTest.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from HardwareLibs import RoboHat 4 | 5 | RoboHat.init() 6 | 7 | try: 8 | while True: 9 | dist = RoboHat.getDistance() 10 | print "Distance: ", int(dist) 11 | time.sleep(.1) 12 | 13 | except KeyboardInterrupt: 14 | print 15 | pass 16 | 17 | finally: 18 | RoboHat.cleanup() 19 | -------------------------------------------------------------------------------- /Examples/ssonarTest.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from HardwareLibs import RoboHat 4 | 5 | RoboHat.init() 6 | 7 | try: 8 | while True: 9 | dist = RoboHat.getDistance() 10 | print "Distance: ", int(dist) 11 | time.sleep(1) 12 | 13 | except KeyboardInterrupt: 14 | print 15 | pass 16 | 17 | finally: 18 | RoboHat.cleanup() 19 | -------------------------------------------------------------------------------- /HardwareLibs/Camera.py: -------------------------------------------------------------------------------- 1 | from HardwareLibs.RoboHat import startServos, stopServos, setServo 2 | from picamera.array import PiRGBArray 3 | from picamera import PiCamera 4 | from threading import Thread 5 | from Constants import panOffset 6 | import cv2 7 | 8 | 9 | class PiVideoStream: 10 | def __init__(self, resolution=(640, 480), framerate=32): 11 | # initialize the camera and stream 12 | self.camera = PiCamera() 13 | self.camera.resolution = resolution 14 | self.camera.framerate = framerate 15 | self.resolution = resolution 16 | 17 | self.rawCapture = PiRGBArray(self.camera, size=resolution) 18 | self.stream = self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True) 19 | 20 | # Threading variables 21 | self.stopped = False 22 | 23 | # Frame variables 24 | self.frame = None 25 | self.frameID = 0 26 | # self.h = 0 27 | # self.w = 0 28 | 29 | 30 | def start(self): 31 | # Start the thread to read frames from the video stream 32 | Thread(target=self.update, args=()).start() 33 | 34 | 35 | # Wait until there is frame loaded 36 | from time import sleep 37 | while self.frame is None: sleep(0.01) 38 | 39 | return self 40 | 41 | def update(self): 42 | # rotationMatrix = None 43 | 44 | # Keep looping infinitely until the thread is stopped 45 | for frame in self.stream: 46 | frame = frame.array 47 | 48 | # # FIRST RUN ONLY: Get basic information about the frame, and the rotation matrix 49 | # if rotationMatrix is None: 50 | # self.h, self.w = frame.shape[:2] 51 | # center = (self.w / 2, self.h / 2) 52 | # rotationMatrix = cv2.getRotationMatrix2D(center, 180, 1.0) 53 | 54 | # Rotate the picture so it's oriented correctly 55 | frame = cv2.flip(frame, 1) 56 | frame = cv2.flip(frame, 0) 57 | # frame = cv2.warpAffine(frame, rotationMatrix, (self.w, self.h)) 58 | 59 | self.frame = frame 60 | self.frameID += 1 61 | self.rawCapture.truncate(0) 62 | 63 | # If the thread indicator variable is set, stop the thread and resource camera resources 64 | if self.stopped: 65 | self.stream.close() 66 | self.rawCapture.close() 67 | self.camera.close() 68 | return 69 | 70 | def read(self): 71 | # Return the frame most recently read 72 | return self.frame 73 | 74 | def close(self): 75 | # Indicate that the thread should be stopped 76 | print("PiVideoStream| Closing Video Thread") 77 | self.stopped = True 78 | 79 | 80 | class PanTiltPiCamera(PiVideoStream): 81 | def __init__(self, panPin, tiltPin): 82 | super().__init__() 83 | super().start() 84 | 85 | self.panPin = panPin 86 | self.tltPin = tiltPin 87 | 88 | startServos() 89 | 90 | # Set servos to the initial position 91 | self.panRot = -1 92 | self.tltRot = -1 93 | self.setPose(pan = 0, tilt = -50) 94 | 95 | def setPose(self, pan=None, tilt=None): 96 | pan += panOffset 97 | 98 | if pan is not None and pan != self.panRot: 99 | setServo(self.panPin, pan) 100 | self.panRot = pan 101 | 102 | if tilt is not None and tilt != self.tltRot: 103 | setServo(self.tltPin, tilt) 104 | self.tltRot = tilt 105 | 106 | 107 | def close(self): 108 | super().close() 109 | print("PanTiltPiCamera| Stopping Servos") 110 | stopServos() 111 | -------------------------------------------------------------------------------- /HardwareLibs/RoboHat.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # 3 | # Python Module to externalise all Initio/RoboHAT specific hardware 4 | # 5 | # Created by Gareth Davies, Feb 2016 6 | # Copyright 4tronix 7 | # 8 | # This code is in the public domain and may be freely copied and used 9 | # No warranty is provided or implied 10 | # 11 | # ====================================================================== 12 | 13 | 14 | # ====================================================================== 15 | # General Functions 16 | # 17 | # init(). Initialises GPIO pins, switches motors off, etc 18 | # cleanup(). Sets all motors off and sets GPIO to standard values 19 | # version(). Returns 2. Invalid until after init() has been called 20 | # ====================================================================== 21 | 22 | 23 | # ====================================================================== 24 | # Motor Functions 25 | # 26 | # stop(): Stops both motors 27 | # forward(speed): Sets both motors to move forward at speed. 0 <= speed <= 100 28 | # reverse(speed): Sets both motors to reverse at speed. 0 <= speed <= 100 29 | # spinLeft(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 30 | # spinRight(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 31 | # turnForward(leftSpeed, rightSpeed): Moves forwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 32 | # turnreverse(leftSpeed, rightSpeed): Moves backwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 33 | # ====================================================================== 34 | 35 | 36 | # ====================================================================== 37 | # IR Sensor Functions 38 | # 39 | # irLeft(): Returns state of Left IR Obstacle sensor 40 | # irRight(): Returns state of Right IR Obstacle sensor 41 | # irAll(): Returns true if either of the Obstacle sensors are triggered 42 | # irLeftLine(): Returns state of Left IR Line sensor 43 | # irRightLine(): Returns state of Right IR Line sensor 44 | # ====================================================================== 45 | 46 | 47 | # ====================================================================== 48 | # UltraSonic Functions 49 | # 50 | # getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object 51 | # ====================================================================== 52 | 53 | # ====================================================================== 54 | # Servo Functions 55 | # 56 | # startServos(). Initialises the servo background process 57 | # stop Servos(). terminates the servo background process 58 | # setServo(Servo, Degrees). Sets the servo to position in degrees -90 to +90 59 | # ====================================================================== 60 | 61 | 62 | # Import all necessary libraries 63 | import RPi.GPIO as GPIO, sys, threading, time, os, subprocess 64 | 65 | # Pins 35, 36 Left Motor 66 | # Pins 32, 33 Right Motor 67 | L1 = 36 68 | L2 = 35 69 | R1 = 33 70 | R2 = 32 71 | 72 | # Define obstacle sensors and line sensors 73 | # These can be on any input pins, but this library assumes the following layout 74 | # which matches the build instructions 75 | irFL = 7 76 | irFR = 11 77 | lineLeft = 29 78 | lineRight = 13 79 | 80 | # Define Sonar Pin (Uses same pin for both Ping and Echo) 81 | sonar = 38 82 | 83 | ServosActive = False 84 | 85 | 86 | # ====================================================================== 87 | # General Functions 88 | # 89 | # init(). Initialises GPIO pins, switches motors and LEDs Off, etc 90 | def init(): 91 | global p, q, a, b 92 | 93 | GPIO.setwarnings(False) 94 | 95 | # use physical pin numbering 96 | GPIO.setmode(GPIO.BOARD) 97 | # print GPIO.RPI_REVISION 98 | 99 | # set up digital line detectors as inputs 100 | GPIO.setup(lineRight, GPIO.IN) # Right line sensor 101 | GPIO.setup(lineLeft, GPIO.IN) # Left line sensor 102 | 103 | # Set up IR obstacle sensors as inputs 104 | GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor 105 | GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor 106 | 107 | # p L1 108 | # q L2 109 | # a R1 110 | # b #R2 111 | 112 | # use pwm on inputs so motors don't go too fast 113 | GPIO.setup(L1, GPIO.OUT) 114 | p = GPIO.PWM(L1, 20) 115 | p.start(0) 116 | 117 | GPIO.setup(L2, GPIO.OUT) 118 | q = GPIO.PWM(L2, 20) 119 | q.start(0) 120 | 121 | GPIO.setup(R1, GPIO.OUT) 122 | a = GPIO.PWM(R1, 20) 123 | a.start(0) 124 | 125 | GPIO.setup(R2, GPIO.OUT) 126 | b = GPIO.PWM(R2, 20) 127 | b.start(0) 128 | 129 | startServos() 130 | 131 | 132 | # cleanup(). Sets all motors off and sets GPIO to standard values 133 | def cleanup(): 134 | stop() 135 | stopServos() 136 | GPIO.cleanup() 137 | 138 | 139 | # version(). Returns 2. Invalid until after init() has been called 140 | def version(): 141 | return 2 # (version 1 is Pirocon, version 2 is RoboHAT) 142 | 143 | 144 | # End of General Functions 145 | # ====================================================================== 146 | 147 | 148 | # ====================================================================== 149 | # Motor Functions 150 | # 151 | # stop(): Stops both motors 152 | def stop(): 153 | p.ChangeDutyCycle(0) 154 | q.ChangeDutyCycle(0) 155 | a.ChangeDutyCycle(0) 156 | b.ChangeDutyCycle(0) 157 | 158 | 159 | 160 | # forward(speed): Sets both motors to move forward at speed. 0 <= speed <= 100 161 | def forward(speed): 162 | p.ChangeDutyCycle(speed) 163 | q.ChangeDutyCycle(0) 164 | a.ChangeDutyCycle(speed) 165 | b.ChangeDutyCycle(0) 166 | p.ChangeFrequency(speed + 5) 167 | a.ChangeFrequency(speed + 5) 168 | 169 | 170 | # reverse(speed): Sets both motors to reverse at speed. 0 <= speed <= 100 171 | def reverse(speed): 172 | p.ChangeDutyCycle(0) 173 | q.ChangeDutyCycle(speed) 174 | a.ChangeDutyCycle(0) 175 | b.ChangeDutyCycle(speed) 176 | q.ChangeFrequency(speed + 5) 177 | b.ChangeFrequency(speed + 5) 178 | 179 | 180 | # spinLeft(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 181 | def spinLeft(speed): 182 | p.ChangeDutyCycle(0) 183 | q.ChangeDutyCycle(speed) 184 | a.ChangeDutyCycle(speed) 185 | b.ChangeDutyCycle(0) 186 | q.ChangeFrequency(speed + 5) 187 | a.ChangeFrequency(speed + 5) 188 | 189 | 190 | # spinRight(speed): Sets motors to turn opposite directions at speed. 0 <= speed <= 100 191 | def spinRight(speed): 192 | p.ChangeDutyCycle(speed) 193 | q.ChangeDutyCycle(0) 194 | a.ChangeDutyCycle(0) 195 | b.ChangeDutyCycle(speed) 196 | p.ChangeFrequency(speed + 5) 197 | b.ChangeFrequency(speed + 5) 198 | 199 | 200 | # turnForward(leftSpeed, rightSpeed): Moves forwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 201 | def turnForward(leftSpeed, rightSpeed): 202 | p.ChangeDutyCycle(leftSpeed) 203 | q.ChangeDutyCycle(0) 204 | a.ChangeDutyCycle(rightSpeed) 205 | b.ChangeDutyCycle(0) 206 | p.ChangeFrequency(leftSpeed + 5) 207 | a.ChangeFrequency(rightSpeed + 5) 208 | 209 | 210 | # turnReverse(leftSpeed, rightSpeed): Moves backwards in an arc by setting different speeds. 0 <= leftSpeed,rightSpeed <= 100 211 | def turnReverse(leftSpeed, rightSpeed): 212 | p.ChangeDutyCycle(0) 213 | q.ChangeDutyCycle(leftSpeed) 214 | a.ChangeDutyCycle(0) 215 | b.ChangeDutyCycle(rightSpeed) 216 | q.ChangeFrequency(leftSpeed + 5) 217 | b.ChangeFrequency(rightSpeed + 5) 218 | 219 | 220 | # End of Motor Functions 221 | # ====================================================================== 222 | 223 | 224 | # ====================================================================== 225 | # IR Sensor Functions 226 | # 227 | # irLeft(): Returns state of Left IR Obstacle sensor 228 | def irLeft(): 229 | if GPIO.input(irFL) == 0: 230 | return True 231 | else: 232 | return False 233 | 234 | 235 | # irRight(): Returns state of Right IR Obstacle sensor 236 | def irRight(): 237 | if GPIO.input(irFR) == 0: 238 | return True 239 | else: 240 | return False 241 | 242 | 243 | # irAll(): Returns true if any of the Obstacle sensors are triggered 244 | def irAll(): 245 | if GPIO.input(irFL) == 0 or GPIO.input(irFR) == 0: 246 | return True 247 | else: 248 | return False 249 | 250 | 251 | # irLeftLine(): Returns state of Left IR Line sensor 252 | def irLeftLine(): 253 | if GPIO.input(lineLeft) == 0: 254 | return True 255 | else: 256 | return False 257 | 258 | 259 | # irRightLine(): Returns state of Right IR Line sensor 260 | def irRightLine(): 261 | if GPIO.input(lineRight) == 0: 262 | return True 263 | else: 264 | return False 265 | 266 | 267 | # End of IR Sensor Functions 268 | # ====================================================================== 269 | 270 | 271 | # ====================================================================== 272 | # UltraSonic Functions 273 | # 274 | # getDistance(). Returns the distance in cm to the nearest reflecting object. 0 == no object 275 | def getDistance(): 276 | GPIO.setup(sonar, GPIO.OUT) 277 | # Send 10us pulse to trigger 278 | GPIO.output(sonar, True) 279 | time.sleep(0.00001) 280 | GPIO.output(sonar, False) 281 | 282 | start = time.time() 283 | count = time.time() 284 | GPIO.setup(sonar, GPIO.IN) 285 | while GPIO.input(sonar) == 0 and time.time() - count < 0.1: 286 | start = time.time() 287 | 288 | count = time.time() 289 | stop = count 290 | while GPIO.input(sonar) == 1 and time.time() - count < 0.1: 291 | stop = time.time() 292 | 293 | # Calculate pulse length 294 | elapsed = stop - start 295 | # Distance pulse travelled in that time is time 296 | # multiplied by the speed of sound (cm/s) 297 | distance = elapsed * 34000 298 | # That was the distance there and back so halve the value 299 | distance = distance / 2 300 | return distance 301 | 302 | 303 | # End of UltraSonic Functions 304 | # ====================================================================== 305 | 306 | # ====================================================================== 307 | # Servo Functions 308 | # Pirocon/Microcon/RoboHAT use ServoD to control servos 309 | 310 | 311 | def setServo(Servo, Degrees): 312 | global ServosActive 313 | # print "ServosActive:", ServosActive 314 | # print "Setting servo" 315 | if ServosActive == False: 316 | startServos() 317 | pinServod(Servo, Degrees) # for now, simply pass on the input values 318 | 319 | 320 | def stopServos(): 321 | stopServod() 322 | 323 | 324 | def startServos(): 325 | # print "Starting servod as CPU =", CPU 326 | startServod() 327 | 328 | 329 | def startServod(): 330 | global ServosActive 331 | # print "Starting servod. ServosActive:", ServosActive 332 | SCRIPTPATH = os.path.split(os.path.realpath(__file__))[0] 333 | os.system("sudo pkill -f servod") 334 | initString = "sudo " + SCRIPTPATH + '/servod --pcm --idle-timeout=20000 --p1pins="18,22" > /dev/null' 335 | 336 | os.system(initString) 337 | ServosActive = True 338 | 339 | 340 | def pinServod(pin, degrees): 341 | # print pin, degrees 342 | pinString = "echo " + str(pin) + "=" + str(50 + ((90 - degrees) * 200 / 180)) + " > /dev/servoblaster" 343 | os.system(pinString) 344 | 345 | 346 | def stopServod(): 347 | global ServosActive 348 | os.system("sudo pkill -f servod") 349 | ServosActive = False 350 | -------------------------------------------------------------------------------- /HardwareLibs/Rover.py: -------------------------------------------------------------------------------- 1 | from threading import Thread, RLock 2 | from time import sleep 3 | 4 | import Constants as Const 5 | from Behaviors.LineBehavior import FollowLine 6 | from HardwareLibs import RoboHat 7 | from HardwareLibs.Camera import PanTiltPiCamera 8 | from HardwareLibs.Wheel import Wheel 9 | 10 | 11 | class RoverHandler: 12 | """ 13 | Initializes and starts a thread where it loops over sensors and logs data as it comes in. 14 | """ 15 | 16 | def __init__(self): 17 | RoboHat.init() 18 | 19 | self.actionLock = RLock() 20 | 21 | # Hardware 22 | self.LWheel = Wheel(Const.leftWheelPinA, 23 | Const.leftWheelPinB, 24 | Const.leftEncoderPinA, 25 | Const.leftEncoderPinB) 26 | 27 | self.RWheel = Wheel(Const.rightWheelPinA, 28 | Const.rightWheelPinB, 29 | Const.rightEncoderPinA, 30 | Const.rightEncoderPinB) 31 | 32 | self.camera = PanTiltPiCamera(Const.cameraPanPin, Const.cameraTiltPin) 33 | 34 | # Behaviors 35 | self.behavior = FollowLine(self) 36 | 37 | # Threading 38 | self.stopped = False 39 | # self.mainThread = Thread(target=self.mainThread) 40 | # self.mainThread.start() 41 | 42 | def mainThread(self): 43 | while not self.stopped: 44 | sleep(.001) # Let other threads do stuff 45 | 46 | with self.actionLock: 47 | # Do Hardware Updates 48 | self.LWheel.update() 49 | self.RWheel.update() 50 | 51 | # Do Behavior Updates 52 | self.behavior.update() 53 | 54 | 55 | self.close() 56 | 57 | def setMoveRadius(self, speed, radius): 58 | """ 59 | Sets both wheels 60 | :param speed: Positive means forward, negative means backwards, 0 means stop 61 | """ 62 | 63 | if radius == 0: return 64 | 65 | vL = speed * (1 + Const.distBetweenWheels / (2 * radius)) 66 | vR = speed * (1 - Const.distBetweenWheels / (2 * radius)) 67 | 68 | print("vL ", vL, "\tvR", vR) 69 | 70 | with self.actionLock: 71 | self.LWheel.setSpeed(vL) 72 | self.RWheel.setSpeed(vR) 73 | 74 | def close(self): 75 | # Run this when ending the main python script 76 | print("Robot| Closing Robot Thread") 77 | 78 | # Safely close main threads 79 | # self.stopped = True 80 | # self.mainThread.join(2) 81 | 82 | 83 | # In case the thread didn't close, use the lock when closing up 84 | with self.actionLock: 85 | self.LWheel.close() 86 | self.RWheel.close() 87 | self.camera.close() 88 | 89 | RoboHat.cleanup() 90 | 91 | 92 | -------------------------------------------------------------------------------- /HardwareLibs/Wheel.py: -------------------------------------------------------------------------------- 1 | import Constants 2 | import RPi.GPIO as GPIO 3 | from collections import namedtuple 4 | from time import time 5 | from Utils import clamp, sign 6 | 7 | 8 | # This is so that wheel logs have identical time scales 9 | global startTime 10 | startTime = time() 11 | getRunTime = lambda: time() - startTime 12 | 13 | 14 | class TimedHardwareLoop: 15 | """ 16 | This will help classes that are in some main loop that need an "update" function of some sort that depends on time. 17 | They can check if it's time to run or not. 18 | """ 19 | 20 | def __init__(self, delay): 21 | self.delay = delay 22 | self.lastTime = 0 23 | 24 | # Keep track of how long the delay ACTUALLY is 25 | self.lastDelay = delay 26 | 27 | def isUpdate(self): 28 | """ 29 | Check if it's time to update 30 | :return: True if ready, False if wait 31 | """ 32 | now = getRunTime() 33 | willRun = now > self.lastTime + self.delay 34 | 35 | if willRun: 36 | self.lastDelay = now - self.lastTime 37 | self.lastTime = now 38 | 39 | return willRun 40 | 41 | def update(self): 42 | # In case the child doesn't have this function 43 | pass 44 | 45 | 46 | class Wheel(TimedHardwareLoop): 47 | """ 48 | A wheel function holds an encoder object, but has the ability to 49 | adjust the 'speed' of the wheel. The Wheel should be run inside 50 | a loop, where 'update' is called every so often. 51 | 52 | The idea of this class is that you can keep track of the wheel 53 | speed and adjust it on the fly. 54 | """ 55 | 56 | 57 | def __init__(self, wheelPinA, wheelPinB, encoderPinA, encoderPinB): 58 | super().__init__(delay=0.05) 59 | 60 | # Set up Wheel Controls 61 | self.speed = 0 62 | self.power = 0 63 | self.lastError = 0 # Last error 64 | 65 | # Set up Wheel Hardware 66 | self.encoder = Encoder(encoderPinA, encoderPinB) 67 | 68 | GPIO.setup(wheelPinA, GPIO.OUT) 69 | self.A_PWM = GPIO.PWM(wheelPinA, 20) 70 | self.A_PWM.start(0) 71 | 72 | GPIO.setup(wheelPinB, GPIO.OUT) 73 | self.B_PWM = GPIO.PWM(wheelPinB, 20) 74 | self.B_PWM.start(0) 75 | 76 | def setSpeed(self, speed, relative=False): 77 | """ 78 | Set the speed goal of the wheel, in mm/s 79 | :param speed: Speed in mm/s 80 | """ 81 | if relative: 82 | self.speed += speed 83 | else: 84 | self.speed = speed 85 | 86 | 87 | 88 | # Kickstart the motor so that there's some velocity values and tick responses 89 | minUnit = 25 90 | if abs(self.power) < minUnit: 91 | 92 | if speed > 0: self.setPower(minUnit) 93 | if speed < 0: self.setPower(-minUnit) 94 | 95 | def setPower(self, power): 96 | """ 97 | Set the power to the motor 98 | :param power: A value from 0 to 100 99 | """ 100 | 101 | # Sanitize power values 102 | power = clamp(power, -100, 100) 103 | 104 | if int(power) == self.power: return # Avoid repeat commands 105 | 106 | 107 | self.power = power 108 | 109 | # Set motor PWMs 110 | if power > 0: 111 | self.A_PWM.ChangeDutyCycle(power) 112 | self.B_PWM.ChangeDutyCycle(0) 113 | self.A_PWM.ChangeFrequency(power + 5) 114 | 115 | if power < 0: 116 | power = abs(power) 117 | self.A_PWM.ChangeDutyCycle(0) 118 | self.B_PWM.ChangeDutyCycle(power) 119 | self.B_PWM.ChangeFrequency(power + 5) 120 | 121 | if power == 0: 122 | self.A_PWM.ChangeDutyCycle(0) 123 | self.B_PWM.ChangeDutyCycle(0) 124 | 125 | def getSpeed(self): 126 | return self.encoder.getVelocity() 127 | 128 | def update(self): 129 | """ 130 | This function runs whenever the encoder on the wheel has an updated tick 131 | :return: 132 | """ 133 | 134 | if not self.isUpdate(): return 135 | 136 | # Constants 137 | # maxPowerChange = 50 * self.delay # Power Change / Seconds 138 | # Works, but slow 139 | kP = 0.005 140 | kD = 0.025 141 | 142 | # Other Options: 143 | kP = 0.01 144 | kD = 0.05 145 | 146 | # Fast Options: 147 | kP = 0.1 148 | kD = 0.05 149 | 150 | # Get the change in power necessary 151 | velocity = self.encoder.getVelocity() 152 | error = self.speed - velocity 153 | errChange = error - self.lastError 154 | 155 | pwrChange = kP * error + kD * errChange 156 | # pwrChange = clamp(pwrChange, -maxPowerChange, maxPowerChange) 157 | 158 | # Set the power 159 | self.setPower(self.power + pwrChange) 160 | self.lastError = error 161 | 162 | # print("T: ", round(getRunTime(), 4), 163 | # "\tLast Delay: ", round(self.lastDelay, 4), 164 | # "\tChange: ", round(pwrChange, 1), 165 | # "\tPwr: ", round(self.power, 2), 166 | # "\tVel: ", round(velocity, 0), 167 | # "\tkP: ", round(kP*error, 3), 168 | # "\tkD: ", round(kD*errChange, 3)) 169 | 170 | def close(self): 171 | # Close main thread and close encoder events 172 | self.encoder.close() 173 | 174 | 175 | class Encoder: 176 | """ 177 | When Speed is: 178 | Positive 179 | 11 180 | 10 181 | 00 182 | 01 183 | 11 184 | 185 | Negative 186 | 11 187 | 01 188 | 00 189 | 10 190 | 11 191 | """ 192 | LogEntry = namedtuple("LogEntry", ["A", "B", "time", "count"]) 193 | 194 | # State Variables 195 | A = 0 196 | B = 0 197 | time = getRunTime() 198 | count = 0 199 | 200 | def __init__(self, pinA, pinB): 201 | """ 202 | 203 | :param pinA: GPIO Pin for Encoder 204 | :param pinB: GPIO Pin for Encoder 205 | :param onPinUpdate: Function that will be called after any pin update 206 | """ 207 | 208 | # Set up basic globals 209 | self.pinA = pinA 210 | self.pinB = pinB 211 | 212 | # This lookup table returns 1 if the motor is moving forward, 0 if backward, depending on pin logs 213 | # (prev A, prev B, curr A, curc B) 214 | self.getDir = {(1, 1, 1, 0): 1, # Backward direction 215 | (1, 0, 0, 0): 1, 216 | (0, 0, 0, 1): 1, 217 | (0, 1, 1, 1): 1, 218 | (1, 1, 0, 1): -1, # Forward direction 219 | (0, 1, 0, 0): -1, 220 | (0, 0, 1, 0): -1, 221 | (1, 0, 1, 1): -1} 222 | 223 | 224 | # Set up GPIO Pins 225 | GPIO.setup(self.pinA, GPIO.IN) 226 | GPIO.setup(self.pinB, GPIO.IN) 227 | 228 | # Get current GPIO Values 229 | self.log = [] # [(pA, pB), (pA, pB)] 230 | self.A = GPIO.input(self.pinA) 231 | self.B = GPIO.input(self.pinB) 232 | firstEntry = self.LogEntry(A=self.A, 233 | B=self.B, 234 | time=getRunTime(), 235 | count=0) 236 | 237 | self.log.append(firstEntry) 238 | 239 | # Set up GPIO Events (after having gotten the values!) High bouncetime causes issues. 240 | GPIO.add_event_detect(pinA, GPIO.BOTH, callback=self.pinChangeEvent, bouncetime=1) 241 | GPIO.add_event_detect(pinB, GPIO.BOTH, callback=self.pinChangeEvent, bouncetime=1) 242 | 243 | def pinChangeEvent(self, pin): 244 | # Find the pin that has been flipped, then act accordingly 245 | newPinA = self.A 246 | newPinB = self.B 247 | 248 | if pin == self.pinA: newPinA = GPIO.input(self.pinA) 249 | if pin == self.pinB: newPinB = GPIO.input(self.pinB) 250 | 251 | 252 | # Check validity and get direction of turn 253 | lookup = (self.A, self.B, newPinA, newPinB) 254 | try: 255 | direction = self.getDir[lookup] 256 | except KeyError: 257 | # print("Encoder| ERROR during lookup: " + str(lookup)) 258 | return 259 | 260 | 261 | # If it's not a full count (AKA 01 or 10 or 1, then skip updating the other info) then update A, B, and leave 262 | if not (newPinA == 0 and newPinB == 0): 263 | self.A = newPinA 264 | self.B = newPinB 265 | return 266 | 267 | # Update State Values 268 | self.A = newPinA 269 | self.B = newPinB 270 | self.time = getRunTime() 271 | self.count += direction 272 | 273 | # Log the current State Values 274 | newEntry = self.LogEntry(A=self.A, 275 | B=self.B, 276 | time=self.time, 277 | count=self.count) 278 | self.log.append(newEntry) 279 | 280 | # Run the Callback Function for the parent 281 | self.getVelocity() 282 | 283 | def getVelocity(self, sampleSize=2): 284 | if len(self.log) < sampleSize + 1: sampleSize = len(self.log) 285 | if sampleSize == 1: return 0 286 | 287 | log = self.log[-sampleSize:] 288 | velocitySum = 0 289 | now = getRunTime() 290 | samples = 0 291 | 292 | 293 | for i in range(0, len(log) - 1): 294 | samples += 1 295 | 296 | old = log[i] 297 | ticks = self.count - old.count 298 | 299 | if ticks == 0: continue 300 | 301 | elapsedTime = now - old.time 302 | timePerTick = elapsedTime / ticks 303 | velocity = Constants.mmPerEncoderTick / timePerTick 304 | velocitySum += velocity 305 | 306 | 307 | return velocitySum / samples 308 | 309 | def close(self): 310 | print("Encoder| Closing Pin Event Threads") 311 | GPIO.remove_event_detect(self.pinA) 312 | GPIO.remove_event_detect(self.pinB) 313 | 314 | -------------------------------------------------------------------------------- /HardwareLibs/servod: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apockill/RoverBot/d4588d817c2e6c74b7d59116a6ed5dc5ad42340a/HardwareLibs/servod -------------------------------------------------------------------------------- /Main.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | 3 | import cv2 4 | import Constants as Const 5 | from HardwareLibs.Rover import RoverHandler 6 | from HardwareLibs.Camera import PanTiltPiCamera 7 | 8 | if __name__ == "__main__": 9 | print("\n\nStarting!\n") 10 | robot = RoverHandler() 11 | robot.mainThread() 12 | 13 | 14 | # print("Final L: ", robot.LWheel.encoder.getVelocity(sampleSize=50)) 15 | # print("Final R: ", robot.RWheel.encoder.getVelocity(sampleSize=50)) 16 | 17 | -------------------------------------------------------------------------------- /Utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cmath 3 | 4 | 5 | def lineAngle(p1, p2): 6 | # Get the angle between a line segment and the horizontal axis, clockwise 7 | return np.rad2deg(np.arctan2(p1[1] - p2[1], p1[0] - p2[0])) 8 | 9 | def clamp(val, low, max): 10 | # Clamp a value between a min and max 11 | 12 | if val < low: 13 | return low 14 | 15 | elif val > max: 16 | return max 17 | 18 | return val 19 | 20 | 21 | def sign(val): 22 | # Return the sign of a value. 0 is positive 23 | if val < 0: 24 | return -1 25 | return 1 26 | 27 | 28 | class FpsTimer: 29 | """ 30 | This module helps keep scripts at a certain FPS. The Interpreter script uses this, as does the VideoThread. 31 | This will effectively decide whether or not to wait, and how much to wait, and time how long a script is taking 32 | inside of a loop. 33 | 34 | Usage example: 35 | 36 | fpsTimer = FpsTimer(fps=24) 37 | 38 | while True: 39 | fpsTimer.wait() 40 | if not fpsTimer.ready(): continue 41 | 42 | ### Script goes here ### 43 | """ 44 | 45 | def __init__(self, fps): 46 | 47 | self.fps = fps 48 | self.stepDelay = (1.0 / float(fps)) 49 | self.lastTime = float(1) # The first time is 1, so the script will always run immediately 50 | self.isReady = False 51 | self.mode = 1 52 | 53 | self.currentFPS = 0 54 | 55 | def wait(self): 56 | elapsedTime = time() - self.lastTime 57 | 58 | 59 | # Check if the current FPS is less than the goal. If so, run immediately 60 | if not elapsedTime == 0: 61 | fps = 1.0 / elapsedTime 62 | if fps < self.fps: 63 | self.currentFPS = fps 64 | self.isReady = True 65 | return 66 | 67 | # Since the current FPS is higher than desired, wait the appropriate amount of time 68 | waitTime = self.stepDelay - elapsedTime 69 | if waitTime > .01: 70 | sleep(waitTime) 71 | self.currentFPS = 1.0 / (time() - self.lastTime) 72 | 73 | # Calculate FPS again 74 | self.isReady = True 75 | return 76 | 77 | def ready(self): 78 | 79 | if self.isReady: 80 | self.lastTime = time() 81 | self.isReady = False 82 | return True 83 | else: 84 | return False 85 | -------------------------------------------------------------------------------- /VisionUtils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | 4 | 5 | def autoCanny(image, sigma=0.33): 6 | # compute the median of the single channel pixel intensities 7 | v = np.median(image) 8 | 9 | # apply automatic Canny edge detection using the computed median 10 | lower = int(max(0, (1.0 - sigma) * v)) 11 | upper = int(min(255, (1.0 + sigma) * v)) 12 | edged = cv2.Canny(image, lower, upper) 13 | 14 | # return the edged image 15 | return edged 16 | 17 | 18 | def isolateColor(img, lower, upper): 19 | """ 20 | :param img: Image to isolate teh color of 21 | :param lower: [lowerHue, lowerSat, lowerVal] 22 | :param upper: [upperHue, upperSat, upperVal] 23 | :return: Isolated image 24 | """ 25 | 26 | hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) 27 | 28 | if lower[0] > upper[0]: 29 | # If the HSV values wrap around, then intelligently mask it 30 | 31 | upper1 = [180, upper[1], upper[2]] 32 | mask1 = cv2.inRange(hsv, np.array(lower), np.array(upper1)) 33 | 34 | lower2 = [0, lower[1], lower[2]] 35 | mask2 = cv2.inRange(hsv, np.array(lower2), np.array(upper)) 36 | 37 | mask = mask1 + mask2 38 | 39 | else: 40 | mask = cv2.inRange(hsv, np.array(lower), np.array(upper)) 41 | 42 | 43 | final = cv2.bitwise_and(img, img, mask=mask) 44 | return final 45 | 46 | -------------------------------------------------------------------------------- /robohat.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apockill/RoverBot/d4588d817c2e6c74b7d59116a6ed5dc5ad42340a/robohat.pyc --------------------------------------------------------------------------------