def arc_right(dist, power=100):
"""
Sweeping turn to the right
:param dist:
:param power:
"""
leftWheel.reset()
rightWheel.reset()
while leftWheel.count < dist:
drive.move(power, power - 20)
drive.stop()
def align_encoder():
"""
Ensure both wheels are aligned on encoders
"""
leftWheel.reset()
while leftWheel.count < 1:
drive.move(40, 0)
drive.stop()
rightWheel.reset()
while rightWheel.count < 1:
drive.move(0, 40)
drive.stop()
def right90():
"""
Turn robot right 90 degrees by running left wheel
"""
dist = 21.0
drive.stop()
rightWheel.reset()
leftWheel.reset()
move_state = True
while move_state:
log("target= {0} v actual= {1}".format(dist, leftWheel.dist))
if dist < leftWheel.dist:
drive.stop()
move_state = False
else:
drive.move(40, 0)
drive.stop()
def left90():
"""
Turn the robot 90 degrees left by running the right hand motor
"""
dist = 19.0
drive.stop()
rightWheel.reset()
leftWheel.reset()
move_state = True
while move_state:
log("target= {0} v actual= {1}".format(dist, rightWheel.dist))
if dist < rightWheel.dist:
drive.stop()
move_state = False
else:
drive.move(0, 40)
drive.stop()
# Put encoder to known position
align_encoder()
def get_speed(client_sock):
"""
Get speed request from Bluetooth remote control
:param client_sock:
:raise ValueError:
"""
speeds = [0, 0]
print "Read"
try:
data = client_sock.recv(1024)
except BluetoothError:
print("Problem with read {0}".format(BluetoothError))
raise ValueError(1)
print 'Blueberry: [%s]' % data.rstrip()
if len(data) > 3:
print len(data)
speeds = re.findall("-?\d+", data)
drive.move(float(speeds[0]), float(speeds[1]))
try:
write_bt(client_sock)
except ValueError, e:
raise ValueError(e)
def move(source, destination):
global directory
if source not in directory.keys():
print("error: invalid source")
return
if destination not in directory.keys():
print("error: invalid destination")
return
if os.path.isfile(destination):
print("error: destination should be a directory")
return
directory = drive.move(source, destination, directory)
def reverse_dist(dist, power):
"""
Travel a given distance in reverse
:param dist:
:param power:
"""
leftWheel.reset()
rightWheel.reset()
while leftWheel.count < dist or rightWheel.count < dist:
log("leftRev={0} vs rightRev={1}".format(leftWheel.count, rightWheel.count))
if leftWheel.count < rightWheel.count:
log("Teak right")
drive.move(-power, 0)
elif leftWheel.count > rightWheel.count:
log("Teak left")
drive.move(0, -power)
else:
log("No tweaks")
drive.move(-power, -power)
drive.stop()
def forward_dist(dist, power=100):
"""
Travel forwards a given distance
:param dist:
:param power:
"""
leftWheel.reset()
rightWheel.reset()
while leftWheel.count < dist or rightWheel.count < dist:
log("leftFwd={0} vs rightFwd={1}".format(leftWheel.count, rightWheel.count))
if leftWheel.count < rightWheel.count:
log("Teak right")
drive.move(power, 0)
elif leftWheel.count > rightWheel.count:
log("Teak left")
drive.move(0, power)
else:
log("No tweaks")
drive.move(power, power)
drive.stop()
log("Distance reached so stopping")
import socket
import time
import sys
from drive import move
TCP_IP = '192.168.2.18'
TCP_PORT = 5005
BUFFER_SIZE = 20 # Normally 1024, but we want fast response
try:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((TCP_IP, TCP_PORT))
s.listen(1)
conn, addr = s.accept()
print('Connection address:', addr)
end_time = time.time() + 300 * 1 # 1 minute
while time.time() < end_time:
data = conn.recv(BUFFER_SIZE)
if data:
move(data)
print("received data:", data)
conn.send(data) # echo
conn.close()
except OSError as osErr:
print(osErr)
except:
print(sys.exc_info()[0])
else:
conn.close()
turnSpeedLeft = turnSpeedRight = 70
# Flash leds to show has started
LEDS.flash_all(10, 0.5)
# initial movement shall be controlled via push button
buttonHandle.wait_for_press()
while True:
# push button
buttonValue = buttonHandle.count
if buttonValue != buttonValuePrev:
if buttonHandle.is_odd():
print "Starting the motors: "
StartStraightLine = 1
drive.move(nominalSpeedLeft, nominalSpeedRight)
elif not buttonHandle.is_odd():
print "Wating to restart robot..."
StartStraightLine = 0
buttonHandle.wait_for_press()
StartStraightLine = 1
buttonValuePrev = buttonValue
if StartStraightLine:
irLeftValue = irLeftHandle.value
irRightValue = irRightHandle.value
## left IR sensor
if irLeftValue != irLeftValuePrev:
if irLeftValue == 1:
LEDS.flash_all(3)
StartProximityDetectionPrev = StartProximityDetection
# press button to start motors
if not StartProximityDetection:
# if not buttonHandle.is_odd():
print "Waiting for button press..."
buttonHandle.wait_for_press()
# print "Starting proximity alert..."
StartProximityDetection = 1
runningOfIr = 0
irLeftValue = irRightValue = 0
irLeftValuePrev = irRightValuePrev = 0
stopLeft = stopRight = 0
drive.move(nominalSpeedLeft, nominalSpeedRight)
# run proximity alert, stop if distance very low
if StartProximityDetection:
distance = proximityHandle.measure_distance()
irLeftValue = irLeftHandle.value
irRightValue = irRightHandle.value
if not runningOfIr:
if distance < distance_threshold and distance_prev < distance_threshold:
# if distance < distance_threshold:
drive.move(slowSpeedLeft, slowSpeedRight)
runningOfIr = 1
else:
if irRightValue != irRightValuePrev:
def on_press(key):
#frame = vs.read()
frame = 'Evangelion'
if key == Key.esc:
drive.stop()
cv2.destroyAllWindows()
#vs.stop()
return False
if key == Key.up:
drive.move(Forward)
print('foward')
printtofile(frame, 'forward')
if key == Key.down:
drive.move(Reverse)
print('reverse')
printtofile(frame, 'reverse')
if key == Key.left:
drive.move(Left)
print('left')
printtofile(frame, 'left')
if key == Key.right:
drive.move(Right)
print('right')
printtofile(frame, 'right')
if key == Key.page_up:
drive.move(F_Right)
print('f_right')
printtofile(frame, 'f_right')
if key == Key.home:
drive.move(F_Left)
print('f_left')
printtofile(frame, 'f_left')
if key == Key.page_down:
drive.move(R_Right)
print('r_right')
printtofile(frame, 'r_right')
if key == Key.end:
drive.move(R_Left)
print('r_left')
printtofile(frame, 'r_left')
if key == Key.f1:
drive.move(Spin_F_Left)
print('spot_f_left')
printtofile(frame, 'spin_f_left')
if key == Key.f2:
drive.move(Spin_F_Right)
print('spin_f_right')
printtofile(frame, 'spin_f_left')
if key == Key.f3:
drive.move(Spin_R_Left)
print('spin_r_left')
printtofile(frame, 'spin_r_left')
if key == Key.f4:
drive.move(Spin_R_Right)
print('spin_r_right')
printtofile(frame, 'spin_r_right')
if key == Key.f5:
drive.move(Spot_F_Right)
print('spot_f_right')
printtofile(frame, 'r_left')
if key == Key.f6:
drive.move(Spot_F_Left)
print('spot_f_left')
printtofile(frame, 'spot_f_left')
if key == Key.f7:
drive.move(Spot_R_Right)
print('spot_r_right')
printtofile(frame, 'spo_r_left')
if key == Key.f8:
drive.move(Spot_R_Left)
print('spot_r_left')
printtofile(frame, 'r_left')
#if key == Key.enter:
# drive.stop()
# print('stop')
# printtofile(frame,'stop')
if key == Key.delete:
drive.stop()
drive.cleanup()
print('STOP AND CLEANUP')
