def wallAvoid():
print('Starting wall avoid')
speed = 100
time_end = time.time() + 4
# main loop
try:
for i in range(2):
time_end = time.time() + 6
while time.time() < time_end:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
else:
robohat.forward(speed)
robohat.forward(0)
time.sleep(3)
robohat.forward(speed)
time.sleep(0.2)
robohat.spinLeft(speed)
time.sleep(4)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
def wallAvoid():
print('Starting wall avoid')
speed = 100
time_end = time.time() + 4
# main loop
try:
for i in range(2):
time_end = time.time() + 6
while time.time() < time_end:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
else:
robohat.forward(speed)
robohat.forward(0)
time.sleep(3)
robohat.forward(speed)
time.sleep(0.2)
robohat.spinLeft(speed)
time.sleep(4)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
def left(revs, speed):
robohat.spinLeft(speed)
clicks = int(revs*CLICKS_PER_REV)
# doesn't matter which side's sensor we use
s = sensors[SENS_L]
s.start(clicks).wait(5)
robohat.stop()
time.sleep(1)
print("final clicks: ", s._sensorClicks)
def updateMotors(self):
Y_axis = self.yValue
X_axis = self.xValue
r_speed, l_speed = self.joystickToDiff(X_axis, -Y_axis, -1, 1, -100,
100)
#print("Left: " + str(l_speed) + ", Right: " + str(r_speed))
if Y_axis < 0:
if X_axis > 0:
if r_speed < 0:
r_speed = 20.0
else:
r_speed = r_speed + 20
r_speed = min(100, r_speed)
robohat.turnForward(l_speed, r_speed)
elif X_axis < 0:
if l_speed < 0:
l_speed = 20.0
else:
l_speed = l_speed + 20
l_speed = min(100, l_speed)
robohat.turnForward(l_speed, r_speed)
else:
robohat.turnForward(l_speed, r_speed)
elif Y_axis > 0:
if X_axis > 0:
r_speed = -r_speed
if l_speed < 0:
l_speed = -l_speed + 20
l_speed = min(100, l_speed)
else:
l_speed = 20.0
robohat.turnReverse(l_speed, r_speed)
elif X_axis < 0:
l_speed = -l_speed
if r_speed < 0:
r_speed = -r_speed + 20
r_speed = max(20, r_speed)
else:
r_speed = 20.0
robohat.turnReverse(l_speed, r_speed)
else:
r_speed = -r_speed
l_speed = -l_speed
robohat.turnReverse(l_speed, r_speed)
else:
if X_axis > 0:
pass
robohat.spinRight(abs(l_speed))
elif X_axis < 0:
pass
robohat.spinLeft(abs(r_speed))
else:
robohat.stop()
def findWall(speed):
s = sensors[SENS_R]
sClicks = s._sensorClicks
robohat.forward(speed)
# Sanity check (possibly report timeout)
countDown = 1000
while(not robohat.irAll() and countDown > 0):
time.sleep(0.01)
countDown -= 1
robohat.stop()
time.sleep(1)
eClicks = s._sensorClicks
print("final clicks: ", (sClicks - eClicks))
def forward(revs, speed):
robohat.forward(speed)
clicks = int(revs*CLICKS_PER_REV)
# doesn't matter which side's sensor we use
s = sensors[SENS_L]
s.start(clicks).wait(5)
# TODO: figure out how many clicks for the motors to stop, and allow for that
# in the above loop:
# speed, extra clicks
# 30, 7-8
# 50, 11-12
# 70, 13
# 100, 13-14
robohat.stop()
time.sleep(1)
print("final clicks: ", s._sensorClicks)
import robohat, time
speed = 80
robohat.init()
# main loop
try:
while True:
rightSensor = robohat.irRight()
leftSensor = robohat.irLeft()
print "right:{0}, left:{1}".format(rightSensor, leftSensor)
if rightSensor and leftSensor:
robohat.spinLeft(speed)
time.sleep(1)
elif rightSensor:
robohat.spinLeft(speed)
time.sleep(0.2)
print('Turn Left')
elif leftSensor == True:
robohat.spinRight(speed)
time.sleep(0.2)
print('Turn Right')
else:
robohat.forward(speed)
except KeyboardInterrupt:
robohat.stop()
robohat.cleanup()
if keyp == 'w' or ord(keyp) == 16:
robohat.forward(speed)
print ('Forward', speed)
elif keyp == 'z' or ord(keyp) == 17:
robohat.reverse(speed)
print ('Reverse', speed)
elif keyp == 's' or ord(keyp) == 18:
robohat.spinRight(speed)
print ('Spin Right', speed)
elif keyp == 'a' or ord(keyp) == 19:
robohat.spinLeft(speed)
print ('Spin Left', speed)
elif keyp == '.' or keyp == '>':
speed = min(100, speed+10)
print ('Speed+', speed)
elif keyp == ',' or keyp == '<':
speed = max (0, speed-10)
print ('Speed-', speed)
elif keyp == ' ':
robohat.stop()
print ('Stop')
elif ord(keyp) == 3:
break
except KeyboardInterrupt:
print
finally:
robohat.cleanup()
def lb(self, value):
robohat.stop()
self.mode = 2
