#!/usr/bin/env python # coding: Latin-1 #autoall27.py - still having problems with LedBorg on/off switch #autoall26.py - continue on LEDBorg light #autoall25.py - need to solve ledborg while loop exit with globals #autoall24.py -Try moving LEDBorg Lights to Lighton/off code section # Creates a web-page interface for DiddyBorg # Import library functions we need import PicoBorgRev import time import sys import threading import SocketServer import picamera import picamera.array import cv2 import UltraBorg import datetime import XLoBorg import math import os import wiringpi as wiringpi wiringpi.wiringPiSetup() #import RPi.GPIO as GPIO # Tell the library to disable diagnostic printouts XLoBorg.printFunction = XLoBorg.NoPrint # Start the XLoBorg module (sets up devices) XLoBorg.Init() # Setup for LedBorg # Setup software PWMs on the GPIO pins PIN_RED = 0 PIN_GREEN = 2 PIN_BLUE = 3 LED_MAX = 100 wiringpi.softPwmCreate(PIN_RED, 0, LED_MAX) wiringpi.softPwmCreate(PIN_GREEN, 0, LED_MAX) wiringpi.softPwmCreate(PIN_BLUE, 0, LED_MAX) wiringpi.softPwmWrite(PIN_RED, 0) wiringpi.softPwmWrite(PIN_GREEN, 0) wiringpi.softPwmWrite(PIN_BLUE, 0) # A function to set the LedBorg colours def SetLedBorg(red, green, blue): wiringpi.softPwmWrite(PIN_RED, int(red * LED_MAX)) wiringpi.softPwmWrite(PIN_GREEN, int(green * LED_MAX)) wiringpi.softPwmWrite(PIN_BLUE, int(blue * LED_MAX)) # A function to turn the LedBorg off def LedBorgOff(): SetLedBorg(0, 0, 0) # Settings Light #GPIO.setmode(GPIO.BOARD) #GPIO.setup(29, GPIO.OUT) #Headlight 1 front right #GPIO.setup(31, GPIO.OUT) #headlight 2 front right #GPIO.setup(33, GPIO.OUT) #Headlight 3 front left #GPIO.setup(35, GPIO.OUT) #Headlight 4 front left #GPIO.setup(32, GPIO.OUT) #Rearlight 1 rear left #GPIO.setup(36, GPIO.OUT) #Rearlight 2 Rear left print 'Init lights, make sure there off' #GPIO.output(29, False) #GPIO.output(31, False) #GPIO.output(33, False) #GPIO.output(35, False) #GPIO.output(32, False) #GPIO.output(36, False) # Settings for the web-page webPort = 80 # Port number for the web-page, 80 is what web-pages normally use imageWidth = 240 # Width of the captured image in pixels imageHeight = 180 # Height of the captured image in pixels frameRate = 10 # Number of images to capture per second displayRate = 2 # Number of images to request per second photoDirectory = '/home/pi' # Directory to save photos to sliderHeight=200 distanceMin = 100.0 # Minimum distance in mm, corresponds to DiddyBorg reversing at 100% distanceMax = 300.0 # Maximum distance in mm, corresponds to DiddyBorg driving at 100% distanceMax2= 500.0 #Maximum distance in mm, corresponds to DiddyBorg driving at 100% # Movement mode constants MANUAL_MODE = 0 # User controlled movement SEMI_AUTO_MODE = 1 # Semi-automatic movement AUTO_MODE = 2 # Fully automatic movement # Global values global PBR global lastFrame global lockFrame global camera global processor global running global watchdog global movementMode global heading #GPIO.setup(40,GPIO.IN) #button pin on switch to shutdown OS borglights = False running = True movementMode = MANUAL_MODE # Class used to implement the web server class WebServer(SocketServer.BaseRequestHandler): def handle(self): global PBR global lastFrame global watchdog global movementMode global borglights borglights = True # Get the HTTP request data reqData = self.request.recv(1024).strip() reqData = reqData.split('\n') # Get the URL requested getPath = '' for line in reqData: if line.startswith('GET'): parts = line.split(' ') getPath = parts[1] break watchdog.event.set() if getPath.startswith('/distances-once'): # Ultrasonic distance readings # Get the readings distance1 = int(UB.GetRawDistance1()) distance2 = int(UB.GetRawDistance2()) distance3 = int(UB.GetRawDistance3()) distance4 = int(UB.GetRawDistance4()) # Read and display the raw magnetometer reading # Build a table for the values httpText = '
' if distance1 == 0: httpText += '' else: httpText += '' % (distance1) if distance2 == 0: httpText += '' else: httpText += '' % (distance2) if distance3 == 0: httpText += '' else: httpText += '' % (distance3) if distance4 == 0: httpText += '' else: httpText += '' % (distance4) httpText += '
None
Front: %04d
None
Left: %04d
None
Right: %04d
None
Rear: %04d
' #httpText += 'Wifi Signal Strength %.0f %%' % (wifiPercent) httpText += '
' self.send(httpText) elif getPath.startswith('/set/'): # Servo position setting: /set/servo/position parts = getPath.split('/') # Get the power levels if len(parts) >= 4: try: servo = int(parts[2]) position = float(parts[3]) except: # Bad values servo = 0 position = 0.0 else: # Bad request servo = 0 position = 0.0 # Ensure settings are within limits if position < -1: position = -1 elif position > 1: position = 1 # Set the new servo position if servo == 1: UB.SetServoPosition1(position) elif servo == 2: UB.SetServoPosition2(position) elif servo == 3: UB.SetServoPosition3(position) elif servo == 4: UB.SetServoPosition4(position) # Read the current servo positions position1 = UB.GetServoPosition1() * 100.0 position2 = UB.GetServoPosition2() * 100.0 position3 = UB.GetServoPosition3() * 100.0 position4 = UB.GetServoPosition4() * 100.0 # Build a table for the values httpText = '
' httpText += '' % (position1) httpText += '' % (position2) httpText += '' % (position3) httpText += '' % (position4) httpText += '
%04d
%04d
%04d
%04d
' self.send(httpText) elif getPath.startswith('/semiAuto'): # Toggle Auto mode if movementMode == SEMI_AUTO_MODE: # We are in semi-auto mode, turn it off movementMode = MANUAL_MODE httpText = '
' httpText += 'Speeds: 0 %, 0 %' httpText += '
' self.send(httpText) PBR.MotorsOff() else: # We are not in semi-auto mode, turn it on movementMode = SEMI_AUTO_MODE httpText = '
' httpText += 'Semi Mode' httpText += '
' self.send(httpText) elif getPath.startswith('/Auto'): # Toggle Auto mode if movementMode == AUTO_MODE: # We are in auto mode, turn it off movementMode = MANUAL_MODE httpText = '
' httpText += 'Speeds: 0 %, 0 %' httpText += '
' self.send(httpText) PBR.MotorsOff() else: # We are not in auto mode, turn it on movementMode = AUTO_MODE httpText = '
' httpText += 'Auto Mode' httpText += '
' self.send(httpText) elif getPath.startswith('/cam.jpg'): # Camera snapshot lockFrame.acquire() sendFrame = lastFrame lockFrame.release() if sendFrame != None: self.send(sendFrame.tostring()) elif getPath.startswith('/Lighton'): # Lights on print 'Lights On',borglights borglights = True #Turn on Headlights #GPIO.output(29, True) #Headlight 1 front right #GPIO.output(31, True) #headlight 2 front right #GPIO.output(33, True) #headlight 3 front left #GPIO.output(35, True) #headlight 4 front left #GPIO.output(32, True) #headlight 1 Rear #GPIO.output(36, True) #headlight 2 Rear httpText = '
' httpText += 'Lights On' httpText += '
' self.send(httpText) print 'borglights', borglights while borglights: print 'borglights at While break test', borglights if not borglights: break # Loop over a set of different hues: print 'Inside While LEDBorg Loop' print 'borglights', borglights for hue in range(30): print 'borglights at break test after for', borglights if not borglights: break print 'hue range', hue, range # Get hue into the 0 to 3 range hue /= 100.0 # Decide which two channels we are between if hue < 1.0: # Red to Green red = 1.0 - hue green = hue blue = 0.0 elif hue < 2.0: # Green to Blue red = 0.0 green = 2.0 - hue blue = hue - 1.0 else: # Blue to Red red = hue - 2.0 green = 0. blue = 3.0 - hue # Set the chosen colour SetLedBorg(red, green, blue) # Wait a short while print 'borglights at break test after sleep', borglights if not borglights: break time.sleep(0.1) time.sleep(0.2) elif getPath.startswith('/Lightoff'): # Lights off print 'lights off borg',borglights borglights = False SetLedBorg(0, 0, 0) #GPIO.output(29, False) #GPIO.output(31, False) #GPIO.output(33, False) #GPIO.output(35, False) #GPIO.output(32, False) #GPIO.output(36, False) httpText = '
' httpText += 'Lights Off' httpText += '
' self.send(httpText) elif getPath.startswith('/off'): # Turn the drives off and switch to manual mode movementMode = MANUAL_MODE httpText = '
' httpText += 'Speeds: 0 %, 0 %' httpText += '
' self.send(httpText) PBR.MotorsOff() borglights = False elif getPath.startswith('/drv/'): # Motor power setting: /set/driveLeft/driveRight parts = getPath.split('/') # Get the power levels if len(parts) >= 4: try: driveLeft = float(parts[2]) driveRight = float(parts[3]) except: # Bad values driveRight = 0.0 driveLeft = 0.0 else: # Bad request driveRight = 0.0 driveLeft = 0.0 # Ensure settings are within limits if driveRight < -1: driveRight = -1 elif driveRight > 1: driveRight = 1 if driveLeft < -1: driveLeft = -1 elif driveLeft > 1: driveLeft = 1 # Report the current settings percentLeft = driveLeft * 100.0; percentRight = driveRight * 100.0; httpText = '
' if movementMode == MANUAL_MODE: httpText += 'Speeds: %.0f %%, %.0f %%' % (percentLeft, percentRight) elif movementMode == SEMI_AUTO_MODE: httpText += 'Semi: %.0f %%, %.0f %%' % (percentLeft, percentRight) elif movementMode == AUTO_MODE: percentLeft = PBR.GetMotor2() * 100.0; percentRight = PBR.GetMotor1() * 100.0; httpText += 'Auto: %.0f %%, %.0f %%' % (percentLeft, percentRight) httpText += '
' self.send(httpText) # Set the outputs as long as we are not in auto mode if movementMode != AUTO_MODE: driveLeft *= maxPower driveRight *= maxPower PBR.SetMotor1(driveRight) PBR.SetMotor2(-driveLeft) elif getPath.startswith('/photo'): # Save camera photo lockFrame.acquire() captureFrame = lastFrame lockFrame.release() httpText = '
' if captureFrame != None: photoName = '%s/Photo %s.jpg' % (photoDirectory, datetime.datetime.utcnow()) try: photoFile = open(photoName, 'wb') photoFile.write(captureFrame) photoFile.close() httpText += 'Photo saved to %s' % (photoName) except: httpText += 'Failed to take photo!' else: httpText += 'Failed to take photo!' httpText += '
' self.send(httpText) elif getPath == '/': # Main page, click buttons to move and to stop httpText = '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '
\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '

\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '

\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '

\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '

\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '

\n' httpText += '
Cam Left/Right      Cam Up/Down                  Servo 3      Servo 4
\n' httpText += '\n' httpText += '\n' httpText += '' httpText += ' ' httpText += ' ' httpText += ' ' httpText += ' ' httpText += ' ' httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += '\n' httpText += '
\n' httpText += '
Distances (mm)
\n' httpText += '\n' httpText += '\n' httpText += '\n' self.send(httpText) elif getPath == '/stream': # Streaming frame, set a delayed refresh displayDelay = int(1000 / displayRate) httpText = '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' % (displayDelay) httpText += '
\n' httpText += '\n' httpText += '\n' self.send(httpText) elif getPath == '/servo': # Alternative page with only servo control, move sliders to change the servo positions httpText = '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '' httpText += ' ' httpText += ' ' httpText += ' ' httpText += ' ' httpText += ' ' httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += ' \n' % (sliderHeight) httpText += '
' httpText += '\n' httpText += '\n' httpText += '\n' self.send(httpText) elif getPath == '/distances': # Repeated reading of the ultrasonic distances, set a delayed refresh # We use AJAX to avoid screen refreshes caused by refreshing a frame displayDelay = int(1000 / displayRate) httpText = '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' httpText += '\n' % (displayDelay) httpText += '
Waiting for first distance reading...
\n' httpText += '\n' httpText += '\n' self.send(httpText) else: # Unexpected page self.send('Path : "%s"' % (getPath))