def step(self, data):
ir_data = [obj[1] for obj in data if obj[0] == "IR"]
ir_val = [ir_data[0][0], ir_data[1][0], ir_data[2][0]]
if log:
for i in xrange(3):
print "IR #" + str(i) + " " + str(ir_val[i]) + " " + str(ir_val[i] <= self.THRESH[i])
if ir_val[0] != None and ir_val[1] != None and ir_val[2] != None:
if not self.IRPid[0].running:
self.IRPid1.start(ir_val[0], self.THRESH[0])
self.IRPid2.start(ir_val[1], self.THRESH[1])
self.IRPid3.start(ir_val[2], self.THRESH[2])
self.PidOut[0] = self.IRPid1.iterate(ir_val[0])
self.PidOut[1] = self.IRPid2.iterate(ir_val[1])
self.PidOut[2] = self.IRPid3.iterate(ir_val[2])
(r, l) = utils.getMotorSpeeds(self.Speed, sum(self.PidOut[1:]) * self.RotationSpeed)
rSpeed = -int(utils.boundAndScale(r, 0, 1.0, 0.01, 8, 127))
lSpeed = -int(utils.boundAndScale(l, 0, 1.0, 0.01, 8, 127))
if log:
print (ir_val, (rSpeed, lSpeed))
print (self.PidOut, sum(self.PidOut[1:]) * self.RotationSpeed)
self.ctl.drive(rSpeed, lSpeed)
"""
def main():
global bt
global mRight
global mLeft
#vision
camAngle = 0
camHeight = 6
camXFov = 67
camYFov = 50
imageHeight = 480
imageWidth = 640
#arduino
ard = arduino.Arduino()
mRight = arduino.Motor(ard, 10, 5, 3)
mLeft = arduino.Motor(ard, 10, 6, 4)
ard.run()
#motion
rotationSpeed = .2
targetSpeed = .1
#pid
myPid = pid.Pid(.03,.005,.005,100)
#state
searchState = 0
huntState = 1
doneState = 2
state = 0
bt.start()
#fps
tLast = time.time()
tAvg = 0
while True:
(r, l) = (0, 0)
loc = bt.update()
if (loc == None):
print "Waiting for camera"
sleep(1)
continue
if state == searchState:
print "search"
(r, l) = utils.getMotorSpeeds(0.0, rotationSpeed)
if (loc != 0):
state = huntState
if state == huntState:
print "hunt"
print loc
y = loc % imageHeight
x = loc / imageHeight
print (x, y)
distance = 0
angle = (x - (imageHeight/2.0)) * camXFov / imageWidth
print angle
if (not myPid.running):
myPid.start(angle, 0)
continue
print 'running'
pidVal = myPid.iterate(angle)
print pidVal
(r, l) = utils.getMotorSpeeds(targetSpeed, rotationSpeed * pidVal)
if (loc != None):
pass
#state = doneState
print (r, l)
r = int(utils.boundAndScale(r, 0, 1.0, .01, 16, 127))
l = int(utils.boundAndScale(l, 0, 1.0, .01, 16, 127))
print (r, l)
mRight.setSpeed(-r)
mLeft.setSpeed(-l)
tCurr = time.time()
tDiff = tCurr - tLast
tLast = tCurr
tAvg = 0.9*tAvg + 0.1*tDiff
print 1/tAvg
RotationSpeed=.2
while True:
# Main loop -- check the sensor and update the digital output\
ir_val = [a1.getValue(),a2.getValue(),a3.getValue()] # Note -- the higher value, the *closer* the dist
for i in xrange(3):
print "IR #" + str(i)+ " " +str(ir_val[i])+ " " + str(ir_val[i]<=THRESH[i])
while ir_val[0]!=None and ir_val [1]!= None and ir_val[2]!=None:
ir_val = [a1.getValue(),a2.getValue(),a3.getValue()] # Note -- the higher value, the *closer* the dist
if (not IRPid[0].running):
IRPid1.start(ir_val[0],THRESH[0])
IRPid2.start(ir_val[1],THRESH[1])
IRPid3.start(ir_val[2],THRESH[2])
PidOut[0]=IRPid1.iterate(ir_val[0])
PidOut[1]=IRPid2.iterate(ir_val[1])
PidOut[2]=IRPid3.iterate(ir_val[2])
(r,l)=utils.getMotorSpeeds(Speed,sum(PidOut[1:])*RotationSpeed)
rSpeed = -int(utils.boundAndScale(r, 0, 1.0, .01, 8, 127))
lSpeed = -int(utils.boundAndScale(l, 0, 1.0, .01, 8, 127))
m0.setSpeed(rSpeed)
m1.setSpeed(lSpeed)
print (ir_val, (rSpeed, lSpeed))
print (PidOut, sum(PidOut[1:])*RotationSpeed)
time.sleep(0.1)