def __init__(self, win_width = 640, win_height = 480):
pygame.init()
self.width = win_width
self.height = win_height
self.screen = pygame.display.set_mode((win_width, win_height))
pygame.display.set_caption("ToF Viewer")
self.clock = pygame.time.Clock()
# Simple depiction of the robot as a rectangle
#width = win_width/4.0
#height = win_height / 2.0
#topX = win_width / 2.0
#topY = win_height * 0.33
#self.robotVertices = [(topX-width*0.66,topY),(topX+width*0.66,topY),(topX+width,topY+height),(topX-width,topY+height),(topX-width*0.66,topY)]
#self.robotColor = (255,64,64)
# Robot image
self.image = pygame.transform.scale(pygame.image.load("display/Dangle_IMG_1351-small.png").convert_alpha(), (320,320))
self.imageRect = self.image.get_rect()
self.lineColor = (255,255,255)
self.labels = ["Left","Forward","Right"]
self.sensors = []
for s in [18,17,16]:
self.sensors.append(SensorAccessFactory.getSingleton().analog(s))
self.distances = [[40,50,60,70,50,40],[40,50,60,70,50,40],[40,50,60,70,50,40]]
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
#self.vision = VisionAccessFactory.getSingleton()
# Common controls
self.grabberControl = GrabberControl()
self.cameraLevellingControl = CameraLevellingControl()
self.zGunControl = ZGunControl()
# Get config
config = Config()
self.pidP = config.get("wall.pid.p", 0.015)
self.pidI = config.get("wall.pid.i", 0.0) #0.001)
self.pidD = config.get("wall.pid.d", 0.0012)
self.proportionalOnMeasure = config.get("wall.pid.pom", False)
self.maxForward = config.get("wall.forward.max", 1.0)
self.maxManualTurn = config.get("wall.manualturn.max", -15.0)
self.maxHeadingTurn = config.get("wall.headingturn.max", 0.5)
self.autoMaxSpeed = config.get("wall.speed", 0.6)
self.cameraTilt = config.get("wall.camera.tilt", -0.4)
self.leftRightMaxDist = config.get("wall.leftright.maxdist", 600)
self.forwardTurnDist = config.get("wall.forward.maxdist", 450)
self.wallDistTarget = config.get(
"wall.targetdist", 305) # 305 ~= 210mm clearance each side
self.wallFollowAdjustmentRate = config.get("wall.move.followfactor",
300.0)
self.wallFollowMaxAngle = config.get("wall.move.maxfollowangle", 15.0)
self.turnAllowedTime = config.get("wall.move.turnspeed", 1.0)
self.turnAheadSpeed = config.get("wall.move.turnaheadspeed", 0.4)
config.save()
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
self.vision = VisionAccessFactory.getSingleton()
# Common controls
self.grabberControl = GrabberControl()
self.cameraLevellingControl = CameraLevellingControl()
self.zGunControl = ZGunControl()
# Get config
config = Config()
self.pidP = config.get("minesweeper.pid.p", 0.015)
self.pidI = config.get("minesweeper.pid.i", 0.0) #0.001)
self.pidD = config.get("minesweeper.pid.d", 0.0012)
self.proportionalOnMeasure = config.get("minesweeper.pid.pom", False)
self.maxForward = config.get("minesweeper.forward.max", 1.0)
self.maxHeadingTurn = config.get("minesweeper.headingturn.max", 0.6)
self.maxManualTurn = config.get("minesweeper.manualturn.max", -15.0)
self.maxFindLightTurn = config.get("minesweeper.findlightturn.max",
-15.0)
self.autoMaxSpeed = config.get("minesweeper.speed", 0.6)
self.cameraTilt = config.get("minesweeper.camera.tilt", 0.22)
self.achievedStopTime = config.get("minesweeper.achieved.stoptime",
5.0) # seconds
self.minDistanceToStop = config.get("minesweeper.achieved.atdistance",
20.0)
self.MoveTimeMin = config.get("minesweeper.movement.mintime",
1.0) # seconds
config.save()
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
self.vision = VisionAccessFactory.getSingleton()
# Get config
config = Config()
self.points = config.get("display.graph.numpoints", 500)
config.save()
self.fig, self.ax = plt.subplots()
self.x = np.arange(0, self.points, 1)
self.lines = \
self.ax.plot(self.x, [0.0]*len(self.x), label="F Joystick")[0], \
self.ax.plot(self.x, [0.0]*len(self.x), label="L motor")[0], \
self.ax.plot(self.x, [0.0]*len(self.x), label="L speed")[0], \
self.ax.plot(self.x, [0.0]*len(self.x), label="R motor")[0], \
self.ax.plot(self.x, [0.0]*len(self.x), label="R speed")[0], \
self.ax.plot(self.x, [0.0]*len(self.x), label="Heading")[0], \
self.ax.plot(self.x, [0.0]*len(self.x), label="Vision")[0]
self.values = \
Scaler(self.sensors.joystickAxis(1), scaling=1.0), \
Scaler(self.controls.motor(2), scaling=-1.0), \
Scaler(self.sensors.rateCounter(0), scaling=0.0012), \
Scaler(self.controls.motor(1), scaling=1.0), \
Scaler(self.sensors.rateCounter(1), scaling=0.0012), \
Scaler(self.sensors.yaw(), scaling=1/180.0), \
Scaler(self.vision.getLineHeading(), scaling=1/180.0)
plt.ylabel('value')
plt.legend(loc=(0.01, 0.75))
def __init__(self, configPrefix="camera"):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
# Get config
config = Config()
self.scaling = config.get(f"{configPrefix}.leveling.scaling", 0.012)
config.save()
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
# Common controls
self.grabberControl = GrabberControl()
self.cameraLevellingControl = CameraLevellingControl()
self.zGunControl = ZGunControl()
def __init__(self): self.controls = ControlAccessFactory.getSingleton() self.sensors = SensorAccessFactory.getSingleton() self.vision = VisionAccessFactory.getSingleton() self.highPriorityProcesses = [] self.medPriorityProcesses = [] self.counter = 0
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
config = Config()
self.min = config.get("grabber.open", -0.75)
self.max = config.get("grabber.closed", 0.25)
self.speed = config.get("grabber.speed", 0.2)
config.save()
def main(self):
with canvas(self.device) as draw:
self.show(draw)
while True:
if self.stopButton.getValue(
) > 0 or SensorAccessFactory.getSingleton().checkWatchdog() <= 0:
return
time.sleep(0.2)
def __init__(self, device, stopButton):
self.count = 0
self.device = device
self.stopButton = stopButton
# Current Yaw reading
self.sensors = SensorAccessFactory.getSingleton()
self.yaw = self.sensors.yaw()
self.pitch = self.sensors.pitch()
def __init__(self, win_width=640, win_height=480):
pygame.init()
self.width = win_width
self.height = win_height
self.screen = pygame.display.set_mode((win_width, win_height))
pygame.display.set_caption("Image Capture & Analysis Viewer")
self.clock = pygame.time.Clock()
self.sensors = SensorAccessFactory.getSingleton()
self.imageAnalysisResult = VisionAccessFactory.getSingleton(
).getImageResult()
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
# Common controls
self.grabberControl = GrabberControl()
self.cameraLevellingControl = CameraLevellingControl()
self.zGunControl = ZGunControl()
# Get config
config = Config()
self.pidP = config.get("manual.pid.p", 0.015)
self.pidI = config.get("manual.pid.i", 0.0) #0.001)
self.pidD = config.get("manual.pid.d", 0.0012)
self.proportionalOnMeasure = config.get("manual.pid.pom", False)
self.maxForward = config.get("manual.forward.max", 1.0)
self.maxManualTurn = config.get("manual.turn.max", -15.0)
self.maxHeadingTurn = config.get("manual.headingturn.max", 0.6)
self.maxSimpleTurn = config.get("manual.simpleturn.max", 0.4)
self.cameraTilt = config.get("manual.camera.tilt", 0.15)
config.save()
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
self.vision = VisionAccessFactory.getSingleton()
# Common controls
self.grabberControl = GrabberControl()
self.cameraLevellingControl = CameraLevellingControl()
self.zGunControl = ZGunControl()
# Get config
config = Config()
self.pidP = config.get("heading.pid.p", 0.06)
self.pidI = config.get("heading.pid.i", 0.001)
self.pidD = config.get("heading.pid.d", 0.004)
self.proportionalOnMeasure = config.get("heading.pid.pom", False)
self.maxForward = config.get("heading.forward.max", 1.0)
self.maxManualTurn = config.get("heading.manualturn.max", -15.0)
self.maxHeadingTurn = config.get("heading.headingturn.max", 0.5)
self.constantSpeed = config.get("lava.speed", 0.7)
self.cameraTilt = config.get("lava.camera.tilt", 0.5)
config.save()
def __init__(self,
resolution,
threshold,
display,
displayGrayscale,
filename,
blinkers,
numSlices,
framerate=30,
ignoreTopSlices=0,
filterRatio=15,
lookahead=0.8,
saveRaw=True):
self.threshold = threshold
self.filename = filename
self.display = display
self.displayGrayscale = displayGrayscale
self.blinkers = blinkers # pixels at either side of top
self.ignoreTopSlices = ignoreTopSlices
self.saveRaw = saveRaw
# Create/overwrite
self.results = LineAnalysisSharedIPC()
self.results.create()
# Define the analysis parameters
self.radius = resolution[
0] // filterRatio #31 # ensure radius is odd and slighly bigger than the white line
self.numSlices = numSlices
self.targetLookaheadRatio = lookahead # % of screen height that we attempt to head towards
# initialize the camera and grab a reference to the raw camera capture
self.camera = PiCamera()
self.camera.resolution = resolution
self.resolution = resolution
self.camera.framerate = framerate
self.rawCapture = PiRGBArray(self.camera, size=self.resolution)
# Current Yaw reading
self.sensors = SensorAccessFactory.getSingleton()
self.yaw = self.sensors.yaw()
def __init__(self):
# Create/overwrite
self.results = LineAnalysisSharedIPC()
self.results.create()
# Define the analysis parameters
self.radius = 160//15#31 # ensure radius is odd and slighly bigger than the white line
self.numSlices = 20
self.targetLookaheadRatio = 0.8 # % of screen height that we attempt to head towards
# initialize the camera and grab a reference to the raw camera capture
self.camera = PiCamera()
self.camera.resolution = (160, 120)
self.camera.framerate = 30
self.rawCapture = PiRGBArray(self.camera, size=self.camera.resolution)
# Current Yaw reading
self.sensors = SensorAccessFactory.getSingleton()
self.yaw = self.sensors.yaw()
# File capture
# Define the codec and create VideoWriter object.The output is stored in 'outpy.avi' file.
self.captureFile = cv2.VideoWriter('visionCapture.mp4', cv2.VideoWriter_fourcc(*'MJPG'), 10, self.camera.resolution)
def __init__(self):
# construct the argument parse and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-v",
"--video",
help="path to the (optional) video file")
args = vars(ap.parse_args())
self.recordedVideo = args.get("video", False)
if self.recordedVideo:
self.videoFilenanme = args["video"]
# Get config
config = Config()
self.debugPrint = config.get("minesweeper.analysis.debugPrint", False)
self.analysis_width = config.get("minesweeper.analysis.imagewidth",
480)
self.blur_radius = config.get("minesweeper.analysis.burrradius", 11)
#self.minSize = config.get("minesweeper.analysis.minsize", 10)
self.minWidth = config.get("minesweeper.analysis.minWidth", 15)
self.minHeight = config.get("minesweeper.analysis.minHeight", 10)
# Scale factors for real-world measures
self.angleAdjustment = config.get("minesweeper.analysis.anglescale",
2.6) #1.3#0.55
# define the lower and upper boundaries of the target
# colour in the HSV color space, then initialize the
useCalibrationColours = config.get(
"minesweeper.analysis.useCalibrationColours", True)
if not useCalibrationColours:
useLEDColours = config.get("minesweeper.analysis.useLEDColours",
False)
if useLEDColours:
# For LED:
self.colourTargetLower = tuple(
config.get("minesweeper.analysis.colourTargetLowerLED",
[155, 24, 200
])) #(165,90,100)#(158,60,90) #(160,128,24)
self.colourTargetUpper = tuple(
config.get("minesweeper.analysis.colourTargetUpperLED",
[175, 255, 255])) #(175,255,255) #(180,255,255)
else:
# For test biscuit tin lid
self.colourTargetLower = tuple(
config.get("minesweeper.analysis.colourTargetLowerTest",
[165, 94, 69]))
self.colourTargetUpper = tuple(
config.get("minesweeper.analysis.colourTargetUpperTest",
[180, 255, 255]))
self.showImage = config.get("minesweeper.display.image", True)
self.trailSize = config.get("minesweeper.display.trail", 25)
self.frameDelayMs = config.get("minesweeper.analysis.frameDelayMs",
20) # delay after each frame analysis
config.save()
# Load calibration values
configCal = Config("calibrationMinesweeper.json")
if useCalibrationColours:
self.colourTargetLower = tuple(
configCal.get("minesweeper.analysis.colourTargetLower", None))
self.colourTargetUpper = tuple(
configCal.get("minesweeper.analysis.colourTargetUpper", None))
self.cameraNearestVisibleDistance = configCal.get(
"distance.analysis.nearest", 130)
self.cameraNearestVisiblePixels = int(
self.cameraNearestVisibleDistance *
3.5) # Rough approximation equivallent!!
self.cameraFurthestVisiblePixel = configCal.get(
"distance.analysis.horizon", 500)
self.cameraHeightDistance = configCal.get(
"distance.analysis.cameraHeight", 170)
calibrationResolution = configCal.get(
"distance.analysis.calibrationResolution", [480, 640])
self.cameraHeightAdjustment = np.sqrt(
self.cameraHeightDistance * self.cameraHeightDistance +
self.cameraNearestVisibleDistance * self.
cameraNearestVisibleDistance) / self.cameraNearestVisibleDistance
# Adjust for the analysis picture resolution being different to the calibrator
self.cameraFurthestVisiblePixel = self.cameraFurthestVisiblePixel * self.analysis_width // calibrationResolution[
1]
# Results IPC
self.resultsIpc = ImageAnalysisSharedIPC()
self.resultsIpc.create()
# Yaw reading accessor
self.sensors = SensorAccessFactory.getSingleton()
self.yawAccessor = self.sensors.yaw()
if selectButton.getValue() > 0:
break
time.sleep(0.05)
# Display screensaver if sensors not running
if sensors.checkWatchdog() <= 0:
DisplayScreenSaver(lastSelected, None)
display.select(lastSelected)
return lastSelected
def Quit(selected, actionParams):
sys.exit()
sensors = SensorAccessFactory.getSingleton()
title = "Select Challenge"
menus = np.array([
[
"Pi Noon",
[[
Run,
[
'python3', 'DangleRun.py', '--challenge',
'ChallengeManualControl'
]
], [DisplayPositions, []]]
],
[
"Escape Route",
[
def __init__(self): self.controls = ControlAccessFactory.getSingleton() self.sensors = SensorAccessFactory.getSingleton()
