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)
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)
