Nachdem ich vor den Sommerferien meinen Seminarkurs erfolgreich präsentiert habe (15 Punkte), möchte ich den Source-Code (Version 1) meines Roboters nun auch hier veröffentlichen. Bitte bedenkt, dass ich auf meinem Raspberry Pi B ein PiFace drauf habe. Mit ein paar Anpassungen müsste mein Code aber auch ohne dieses funktionieren. Des öfteren wurde ich gefragt wie es mit dem Pirol nun weiter gehen soll. Nachdem ich jetzt ein neues Gehäuse als 3D-Druck angefertigt habe, muss ich erstmal die Hardware in das neue Gehäuse bauen. Dafür benötige ich neue Motoren und Räder. Außerdem muss ich die Energieversorgung ein wenig umstellen. Wenn dies dann soweit fertig ist wird auch die Software um einige Funktionen (die ich jetzt aber noch nicht näher erzählen möchte) erweitert. Außerdem arbeite ich zur Zeit an einem Softwareupdate für mein PitRadio
Hier der Source-Code
'''
Created on Jun 10, 2014
@author: Daniel Schwarz
'''
#
#
# Copyright 2014 by Daniel Schwarz
# www.blackit.eu
# Eine weitere Verwendung des Codes bedarf der Genehmigung des Autors
# Daniel.Schwarz@Blackit.eu
#
#
import sys
import control
def Main():
try:
try:
ctrl = control.Control()
ctrl.driveByTentacle()
except:
print(sys.exc_info())
finally:
control.STOP(doExit=True)
if __name__ == '__main__':
Main()
'''
Created on Jun 10, 2014
@author: Daniel Schwarz
'''
#
#
# Copyright 2014 by Daniel Schwarz
# www.blackit.eu
# Eine weitere Verwendung des Codes bedarf der Genehmigung des Autors
# Daniel.Schwarz@Blackit.eu
#
#
import threading, time, json
# import random
class Control(object):
'''
Zentraler Controller-Thread
'''
def __init__(self, cfgFName="pirol.cfg.json"):
'''
Constructor
'''
# self.motorLeft = Motor("left")
# self.motorRight = Motor("right")
self.cfg = {}
try:
cfg_f = open(cfgFName)
if cfg_f:
self.cfg = json.load(cfg_f)
cfg_f.close()
except:
self.cfg = {
"Controller_MaxRunTime_Sec": 30,
"TurnAndGo_BackTime_Sec": 0.7,
"TurnAndGo_TurnTime_Sec": 0.7,
"TurnAndGo_WaitTime_Sec": 1.2
}
self.tentacleLeft = Tentacle("left")
self.tentacleRight = Tentacle("right")
self.motors = Motors(self.cfg)
self.doDrive = True
timeOut_s = self.cfg.get("Controller_MaxRunTime_Sec", 0)
if timeOut_s > 0:
self.timeOut = ControlTimeOut(timeOut_s, self)
self.timeOut.start()
print("Pirol will stop in %s seconds..." % (timeOut_s))
else:
print("Pirol will stop only if you press Control-C ...")
def driveByTentacle(self):
self.motors.go(1, 1)
while self.doDrive:
tlb = self.tentacleLeft.Back()
trb = self.tentacleRight.Back()
if tlb or trb:
# self.motors.stop()
break
tlf = self.tentacleLeft.Front()
trf = self.tentacleRight.Front()
if tlf or trf:
print("tentacles: TLF=%s, TRF=%s" % (tlf, trf))
if tlf and trf:
self.motors.TurnAndGo("left")
continue
if tlf and not trf:
self.motors.TurnAndGo("right")
continue
if not tlf and trf:
self.motors.TurnAndGo("left")
continue
time.sleep(0.3)
self.motors.stop()
class ControlTimeOut(threading.Thread):
def __init__(self, timeout_sec, control):
threading.Thread.__init__(self)
self.control = control
self.timeout_sec = timeout_sec
self.daemon = True
def run(self):
time.sleep(self.timeout_sec)
self.control.doDrive = False
self.control.motors.stop()
class Tentacle(object):
'''
Fuehler
'''
def __init__(self, which):
'''
Constructor
'''
self.which = which
self.inpins = {
'left': {'front': 4, 'back': 5},
'right': {'front': 6, 'back': 7},
}
def Front(self):
inPinIdx = self.inpins[self.which]['front']
return PiFace.input_pins[inPinIdx].value
def Back(self):
inPinIdx = self.inpins[self.which]['back']
return PiFace.input_pins[inPinIdx].value
class Motors(object):
'''
Motor
'''
# def __init__(self, cfgFName="pirol.cfg.json"):
def __init__(self, cfg):
'''
Constructor
'''
self.motorLeft = Motor("left")
self.motorRight = Motor("right")
self.cfg = cfg
# try:
# cfg_f = open(cfgFName)
# if cfg_f:
# self.cfg = json.load(cfg_f)
# cfg_f.close()
# except:
# self.cfg = {
# "TurnAndGo_BackTime_Sec": 0.7,
# "TurnAndGo_TurnTime_Sec": 0.7,
# "TurnAndGo_WaitTime_Sec": 1.2
# }
def stop(self):
self.motorLeft.stop()
self.motorRight.stop()
# switch off the motor and motor bridge relais
PiFace.output_pins[1].value = 0
PiFace.output_pins[0].value = 0
print("Motors.stop(): disabled motors and bridge")
def go(self, speedLeft, speedRight):
# switch on the motor and motor bridge relais
print("Motors.go(%s, %s): enabling motors and bridge"
% (speedLeft, speedRight))
PiFace.output_pins[0].value = 1
PiFace.output_pins[1].value = 1
self.motorLeft.go(speedLeft)
self.motorRight.go(speedRight)
def TurnAndGo(self, direction):
print("Motors.TurnAndGo(%s) ..." % (direction))
# stop
print("Motors.TurnAndGo(%s): stop!" % (direction))
self.stop()
# go back
print("Motors.TurnAndGo(%s): go back!" % (direction))
self.go(-1, -1)
# time.sleep(0.7)
time.sleep( self.cfg["TurnAndGo_BackTime_Sec"] )
# stop
print("Motors.TurnAndGo(%s): stop!" % (direction))
self.stop()
# turn left 90 degrees
print("Motors.TurnAndGo(%s): turn left!" % (direction))
self.go(-1, 1)
# time.sleep(0.5)
time.sleep( self.cfg["TurnAndGo_TurnTime_Sec"] )
# wait...
self.stop()
# time.sleep(1.5)
time.sleep( self.cfg["TurnAndGo_WaitTime_Sec"] )
# go!
self.go(1, 1)
print("Motors.TurnAndGo(%s): going straight again ..." % (direction))
def _report(pfd):
pass
try:
import pifacedigitalio
PiFace = pifacedigitalio.PiFaceDigital() # creates a PiFace Digtal object
report = _report
except:
import piface_digitalio_fake
PiFace = piface_digitalio_fake.PiFaceDigital() # creates a PiFace Digtal stub
report = piface_digitalio_fake.Report
def STOP(doExit = False):
# my_piface.PiFace.output_port.all_off()
PiFace.output_port.all_off()
if doExit:
exit()
def panic(msg):
print(msg)
STOP(True)
class Motor(threading.Thread):
"""
Motor object with its own thread
"""
def __init__(self, side):
"""
Motor object, side is 'left' or 'right'
"""
threading.Thread.__init__(self)
self.firstPin = 3
self.side_s = side
if side == "left":
self.side = 1
self.handicap = 1 # 2
elif side == "right":
self.side = 0
self.handicap = 1
else:
panic("Motor: unknown side '%s'!" % side)
self.pinIdx1 = self.firstPin + self.side * 2
self.pinIdx2 = self.firstPin + self.side * 2 + 1
self.speed = 0
self.speedLock = threading.RLock()
self.started = False
self.daemon = True
def startRunning(self, direction):
"""
go with full speed for forSeconds seconds (e.g. 2.5)
direction: 0 ==>> forward, 1 ==>> backward
"""
pinVal1 = direction * self.handicap
pinVal2 = (direction + 1) % 2 * self.handicap
#--pinVal1old = PiFace.output_pins[self.pinIdx1].value
#--pinVal2old = PiFace.output_pins[self.pinIdx2].value
# my_piface.PiFace.output_pins[self.pinIdx1].value = pinVal1
# my_piface.PiFace.output_pins[self.pinIdx2].value = pinVal2
PiFace.output_pins[self.pinIdx1].value = pinVal1
PiFace.output_pins[self.pinIdx2].value = pinVal2
#-print("Motor.startRunning(): i%d=%s, i%d=%s" % (
#- self.pinIdx1, pinVal1, self.pinIdx2, pinVal2))
def _goFullThrottle(self, direction, forSeconds):
"""
go with full speed for forSeconds seconds (e.g. 2.5)
direction: 0 ==>> forward, 1 ==>> backward
"""
#-pinVal1 = direction
#-pinVal2 = (direction + 1) % 2
#-pinVal1old = PiFace.output_pins[self.pinIdx1].value
#-pinVal2old = PiFace.output_pins[self.pinIdx2].value
#-PiFace.output_pins[self.pinIdx1].value = pinVal1
#-PiFace.output_pins[self.pinIdx2].value = pinVal2
self.startRunning(direction)
time.sleep(forSeconds)
self.stop()
def stop(self):
"""
stop the motor
"""
self._setSpeed(0)
def __stop(self):
"""
stop the motor
"""
# my_piface.PiFace.output_pins[self.pinIdx1].value = 0 ## pinVal1old
# my_piface.PiFace.output_pins[self.pinIdx2].value = 0 ## pinVal2old
PiFace.output_pins[self.pinIdx1].value = 0 ## pinVal1old
PiFace.output_pins[self.pinIdx2].value = 0 ## pinVal2old
# def stop(self):
# """
# stop the motor
# """
# self.go(0, 0)
def go(self, speed):
"""
start going with a certain speed between -1 and 1;
negative speed means going backwards
"""
with self.speedLock:
if not self.started:
self.start()
self.started = True
print("Motor.%s: speed=%s" % (self.side_s, speed))
self._setSpeed(speed)
# def __go(self, direction, speed, secsToGo):
# """
# go slowly
# direction: 0 ==>> forward, 1 ==>> backward
# speed: 0 ==>> stop, 0.5 ==>> half, 1 ==>> full throttle
# """
# if speed < 0 or speed > 1:
# panic("Motor.go: speed (%s) is not between 0 and 1!" % speed)
#
# interval = 0.02 # seconds
# goSecs = interval * speed
# waitSecs = interval * (1 - speed)
# secsGone = 0
# while secsGone < secsToGo:
# self.goFullThrottle(direction, goSecs)
# time.sleep(waitSecs)
# secsGone += goSecs + waitSecs
def _setSpeed(self, speed):
if speed < -1 or speed > 1:
panic("Motor.setSpeed(%s): invalid speed!" % speed)
with self.speedLock:
self.speed = speed
# print("Motor.%s: speed=%s" % (self.side_s, speed))
def _getSpeed(self):
with self.speedLock:
if self.speed >= 0:
direction = 0
else:
direction = 1
return abs(self.speed), direction ##--(self.speed < 0)
def run(self):
interval = 0.2 # seconds
while True:
speed, direction = self._getSpeed()
goSecs = interval * speed
waitSecs = interval * (1 - speed)
self._goFullThrottle(direction, goSecs)
time.sleep(waitSecs)
# secsGone += goSecs + waitSecs
Neueste Kommentare