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

 

Wir nutzen Cookies, um Dir den bestmöglichen Service auf unserer Webseite zu bieten. mehr Informationen

Die Cookie-Einstellungen auf dieser Website sind auf "Cookies zulassen" eingestellt, um das beste Surferlebnis zu ermöglichen. Wenn du diese Website ohne Änderung der Cookie-Einstellungen verwendest oder auf "Akzeptieren" klickst, erklärst du sich damit einverstanden.

Schließen