コード例 #1
0
ファイル: zeppelin.py プロジェクト: nielsdb/zeppelin
    def __init__(self):
        self.GPIO = GPIO()
        self.directionCalculator = direction(100)
        
        #zet de motoren af bij initialisatie
        self.GPIO.output(self.GPIO.motorUpPins[0], True)
        self.GPIO.output(self.GPIO.motorUpPins[1], True)
        
        self.PWM = self.GPIO.PWM(self.GPIO.pwmPin, 100) #100 frequency
        self.PWM.start(0) #duty cycle
        self.sensor = DistanceSensor(self.GPIO)
        
        
        # WiringPiMotor 4 = Up/Down
        self.motorUp =  MotorUp(self.GPIO, self.GPIO.motorUpPins[0],self.GPIO.motorUpPins[1])
        # WiringPiMotor 3 = Left side
        self.motorLeft = WiringPiMotor(self.GPIO.motorLeftPins[0],self.GPIO.motorLeftPins[1])
        # WiringPiMotor 1 = Right side
        self.motorRight = WiringPiMotor(self.GPIO.motorRightPins[0],self.GPIO.motorRightPins[1])
    
        self.heightCorrectionFlag = False
        
        self.pid = PID()
        self.pid.setPoint(80)
        self.realHeight = 0.0
        
        self.moving = False
        self.lengthToGoal = 0
        
        t = threading.Thread(target=self.heightCorrection)
        t.daemon = True
        t.start()
        
        t2 = threading.Thread(target=self.move)
        t2.daemon = True
        t2.start()

        self.goalTablet= None
        self.goalLocation = None
        
        self.location = (0,0)
        self.angle = 0
        
        mq.setZep(self)
コード例 #2
0
ファイル: zeppelin.py プロジェクト: nielsdb/zeppelin
class Zeppelin():
    def __init__(self):
        self.GPIO = GPIO()
        self.directionCalculator = direction(100)
        
        #zet de motoren af bij initialisatie
        self.GPIO.output(self.GPIO.motorUpPins[0], True)
        self.GPIO.output(self.GPIO.motorUpPins[1], True)
        
        self.PWM = self.GPIO.PWM(self.GPIO.pwmPin, 100) #100 frequency
        self.PWM.start(0) #duty cycle
        self.sensor = DistanceSensor(self.GPIO)
        
        
        # WiringPiMotor 4 = Up/Down
        self.motorUp =  MotorUp(self.GPIO, self.GPIO.motorUpPins[0],self.GPIO.motorUpPins[1])
        # WiringPiMotor 3 = Left side
        self.motorLeft = WiringPiMotor(self.GPIO.motorLeftPins[0],self.GPIO.motorLeftPins[1])
        # WiringPiMotor 1 = Right side
        self.motorRight = WiringPiMotor(self.GPIO.motorRightPins[0],self.GPIO.motorRightPins[1])
    
        self.heightCorrectionFlag = False
        
        self.pid = PID()
        self.pid.setPoint(80)
        self.realHeight = 0.0
        
        self.moving = False
        self.lengthToGoal = 0
        
        t = threading.Thread(target=self.heightCorrection)
        t.daemon = True
        t.start()
        
        t2 = threading.Thread(target=self.move)
        t2.daemon = True
        t2.start()

        self.goalTablet= None
        self.goalLocation = None
        
        self.location = (0,0)
        self.angle = 0
        
        mq.setZep(self)
        
    def setLocation(self, l, a):
        if l != None and l[0] >= 0 and l[1] >= 0:
            self.location = l
        if a != None:
            self.angle = a
    
    def updateInformation(self):
        def updateInformationMQ():
            if self.location != None:
                mq.sendLocation(self.location[0],self.location[1])
            if self.realHeight != None:
                mq.sendHeight(int(self.realHeight)*10)#in mm
            if self.angle != None:
                mq.sendAngle(int(self.angle))
            if cvresult != None:
                mq.sendCvResults(cvresult)
        
        print "updating info"
        cam.makePhoto("/dev/shm/autopic.jpg")
        cvresult = sd.detectShapes("/dev/shm/autopic.jpg")
        print "CvResult " + str(cvresult)
        pA = PhotoAnalysis(zip(cvresult[2], cvresult[3]), grid, (864, 648))
        
        if pA.locationCm != None and pA.angle != None:
            self.setLocation(pA.locationCm, pA.angle)
            print "Location " + str(self.location)
            print "Angle " + str(self.angle)
            print "GoalLocation " + str(self.goalLocation)
            print "Height " + str(self.realHeight)
            updateInformationMQ()
            
            if self.goalLocation != None and nearGoal(self.location, self.goalLocation):
                print "NEAR THE GOAL"
                self.moving = False
                if self.goalTablet != None:
                    #zeg tegen iPad: "Ik ben er"
                    askTablet(self.goalTablet)
                    time.sleep(1)
                    
                    #PROBEER DRIE KEER
                    for i in range(3):
                        #trek een foto en lees de QR code
                        decodedQR = cam.makePhotoAndDecodeQRCode("iPad"+str(i)+".jpg")
                        time.sleep(0.23)
                        if not decodedQR:
                            continue #QR code kon niet gedecodeerd worden
                        #zet nieuw goal
                        self.parseQRCommand(decrypt(decodedQR))
                        break
                else:
                    #DOEN WAT GE MOET DOEN BIJ AANKOMEN OP LOCATIE.
                    self.pid.setPoint(0)
                    self.heightCorrectionFlag = False
                    self.stopAllMotors()
        else:
            print "No Location found"
                  
    
    #(2340,3450) in mm
    def moveTo(self, goalCoordinates):
        if (goalCoordinates in locTabDic):
            self.goalTablet = locTabDic[goalCoordinates]
            self.goalLocation = goalCoordinates
            
        else: 
            self.goalTablet = None
            self.goalLocation = goalCoordinates
        self.lengthToGoal = sqrt((self.location[0]-self.goalLocation[0])**2+(self.goalLocation[1]-self.location[1])**2)
        self.moving = True
    
    def move(self):
        while True:
            self.updateInformation()
            if self.moving :
                dx = self.location[0]-self.goalLocation[0]
                dy = self.location[1]-self.goalLocation[1]
#                 togo = degrees(atan(abs(dx)/abs(dy)))
#                 if dx/dy < 0:
#                     togo = -1*togotogo = degrees(atan(abs(dx)/abs(dy)))
                togo = degrees(atan(dx/dy))
                print "angle togo: " + str(togo)
                (PWM1,PWM2) = self.directionCalculator.update(togo,self.angle)
                if sqrt(dx**2 + dy**2) < self.lengthToGoal/3:
                    PWM1 = int(PWM1/-3)
                    PWM2 = int(PWM2/-3)
                if PWM1 >= 0:
                    self.motorLeft.wiringPiMotorForward(PWM1)
                else: self.motorLeft.wiringPiMotorBackward(abs(PWM1))
                
                if PWM2 >= 0:
                    self.motorRight.wiringPiMotorForward(PWM2)
                else: self.motorRight.wiringPiMotorBackward(abs(PWM2))
    
    def parseQRCommand(self, decoded):
        self.goalLocation, self.goalTablet = parseQr(decoded)
    
    def setPower(self, value):
        if value < 30:
            value = 30
        elif value > 50:
            value = 50
        
        self.PWM.ChangeDutyCycle(value)

    def killPower(self):
        self.PWM.ChangeDutyCycle(0)
        
    
    def setHeight(self, targetHeight):
        if targetHeight < 0:
            return
        self.pid.setPoint = targetHeight
        if targetHeight == 0:
            self.setHeightCorrection(False)
        elif targetHeight > 0:
            self.setHeightCorrection(True)
    
    
    #automatische hoogtecorrectie en autopilot
    def toggleHeightCorrection(self):
        self.setHeightCorrection(not self.heightCorrectionFlag)
        
    def setHeightCorrection(self, newHeightCorrection):
        self.heightCorrectionFlag = newHeightCorrection
        if not newHeightCorrection:
            self.motorUp.motorStop()
        
    def heightCorrection(self):
        while True:
            self.realHeight = self.sensor.getMeasurement()     
            if(self.heightCorrectionFlag):
                pidValue = self.pid.update(self.realHeight)
                self.motorUp.motorForward()
                self.setPower(pidValue)
            time.sleep(0.2)
        
    def stopAllMotors(self):
        self.motorUp.motorStop()
        self.motorLeft.wiringPiMotorStop()
        self.motorRight.wiringPiMotorStop()    
    
    def setPid(self, p, i, d):
        self.pid = PID(p, i, d)