def move_till(destination,direction):
#determine whether right or left wall
if direction=='right':
first='right'
second='left'
elif direction=='left':
first='left'
second='left'
#if first open turn right
if dist.check(first):
motion.turn(first)
if callable(motion.forward):
dist.thread_forward(first)
#if front blocked, check for destination
if not check('front'):
motion.stop()
if destination=='qr':
if dist.check_for_qr:
if vision.scan_qr():
wait_for_path(direction)
return True
elif destination=='box' or destination=='block':
if vision.check_for_block():
wait_for_path(direction)
return True
elif destination=='end':
if vision.ground_is_white():
return True
#turn second if nothing found
else:
motion.turn(second)
#call func again until destination reached
return move_till(destination,direction)
def parseServerData(data):
#####################################
# TCP NETWORK SIGNALS
#####################################
if data:
if data != "env":
conn.send(data) # echo
if data == "close":
CONNECTION_MADE = 0
conn.close()
if data == "shutdown":
conn.send("close")
conn.close()
shutdown()
##################################
# OTHER COMMANDS
##################################
if data == "snap":
os.system("raspistill -o test.jpg")
if data == "env":
STR = 'IP: ' + ip_address + 'Lights are ' + lights + "\n" + 'Temperature:' + str(
int(temp)) + 'C' + "\n" + 'Status: ' + STATUS
conn.send(STR)
if data == "a":
motion.left()
if data == "d":
motion.right()
if data == "w":
motion.forward()
if data == "s":
motion.backward()
if data == "x":
motion.stop()
if data:
if data[0] == 'r':
GPIO.setup(10, GPIO.OUT)
rotate = GPIO.PWM(10, 50)
rotate.start(float(data[1:]))
time.sleep(1)
GPIO.cleanup(10)
if data[0] == 't':
GPIO.setup(9, GPIO.OUT)
tilt = GPIO.PWM(9, 50)
tilt.start(float(data[1:]))
time.sleep(1)
GPIO.cleanup(9)
if data == "cool":
disp.begin()
disp.clear()
disp.display()
image = Image.open('test.jpg').resize((disp.width, disp.height),
Image.ANTIALIAS).convert('1')
disp.image(image)
disp.display()
time.sleep(5)
def move():
#measure time elapsed
time=time.time()-time0
#right check
if distances.box_on_right:
motion.turn('right')
path.append(['right',time])
elif distances.right_open:
motion.turn(right)
path.append(['left',time])
motion.forward()
path.append['forward',time]
#front check
elif distances.front_blocked:
if vision.check_for_box():
motion.stop()
#stores box coordinate in path
box_coordinates.append(len(path)-1)
wait_for_path()
elif distances.check_for_qr():
motion.stop()
qr_coordinates.append(len(path)-1)
number_of_boxes_found=len(box_coordinates)
if number_of_boxes_found<2:
motion.turn('left')
else: wait_for_path()
else:
motion.turn('left')
path.append(['left',time])
#define start-end coordinates
if vision.ground_is_white:
if len(start_coordinates)<2:
start_coordinates.append(len(path)-1)
else:
end_coordinates.append(len(path)-1)
if len(start_coordinates)==2:
path.clear()
follow_path=min_path(path_trim(path,start[0],qr_coordinates[0]),path_trim(start[1],qr_coordinates[0]))
time=0
#call method again
move()
def check_for_qr():
#checks if a qr bock is present whithin 35 cm of stand
if dist(upper) < 35:
#goes to the required range kept 1 cm extra as error due to no brakes.
if dist.upper >= 26:
motion.forward()
check_for_qr()
elif dist.upper <= 24:
motion.backward()
#stops if already in range
else:
motion.stop()
return True
def signal_handler(signal, frame):
print('SIGINT RECIEVED')
LCD1602.write(0, 0, ' ')
LCD1602.write(1, 1, ' ')
motion.stop()
disp = Adafruit_SSD1306.SSD1306_128_32(0)
disp.clear()
setColor(0xff0000)
global CONNECTION_MADE
if CONNECTION_MADE:
CONNECTION_MADE = 0
conn.send("close")
conn.close()
global RUN_SERVER
exit(0)
RUN_SERVER = 0
def _handle_odom_timer():
global _ctrl_on, _velocities, _set_speed, _xhat, _yhat, _thetahat
if _odom_on:
odom = Odometry.update(_odom_timer_period, _velocityhat)
print "{}\r".format(odom)
_xhat = odom[0]
_yhat = odom[1]
_thetahat = odom[2]
if _ctrl_on:
x_c, y_c, theta_c = Controller.get_commanded_position()
if _close(_xhat, x_c) and _close(_yhat, y_c) and \
_close(_thetahat, theta_c, tolerance=8):
_ctrl_on = False
print("\r\n*** Reached Set Point within Tolerances ***\r\n")
_motion_timer.stop()
motion.stop()
time.sleep(0.5)
_velocities = (0, 0, 0)
_set_speed = True
_motion_timer.start()
def test_square_calibration(velocity=0.6,
sleep_time=1.5,
M1QPPS=None,
M2QPPS=None,
M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)
# Right
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Up
motion.drive(0, velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Down
motion.drive(0, -velocity, 0)
time.sleep(sleep_time)
motion.stop()
def test_square_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)
# Right
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Up
motion.drive(0, velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Down
motion.drive(0, -velocity, 0)
time.sleep(sleep_time)
motion.stop()
def square():
motion.drive(.5, 0, 0)
time.sleep(1)
motion.stop()
time.sleep(0.5)
motion.drive(0, .5, 0)
time.sleep(1)
motion.stop()
time.sleep(0.5)
motion.drive(-.5, 0, 0)
time.sleep(1)
motion.stop()
time.sleep(0.5)
motion.drive(0, -.5, 0)
time.sleep(1)
motion.stop()
def square(): motion.drive(.5, 0, 0) time.sleep(1) motion.stop() time.sleep(0.5) motion.drive(0, .5, 0) time.sleep(1) motion.stop() time.sleep(0.5) motion.drive(-.5, 0, 0) time.sleep(1) motion.stop() time.sleep(0.5) motion.drive(0, -.5, 0) time.sleep(1) motion.stop()
def _go_home():
# Get current robot position, rounded to 1 decimal place
xhat = round(_xhat, 1)
yhat = round(_yhat, 1)
thetahat = round(_thetahat, 1)
# Set a tolerance
tolerance = 0.1
if abs(xhat) > tolerance:
dt = abs(xhat / _vx)
sign = -1 if xhat > 0 else 1
print("motion.drive({},{},{}) for {} s\r".format(sign*_vx, 0, 0, dt))
motion.drive(sign*_vx, 0, 0)
time.sleep(dt)
motion.stop()
time.sleep(0.5)
if abs(yhat) > tolerance:
dt = abs(yhat / _vy)
sign = -1 if yhat > 0 else 1
print("motion.drive({},{},{}) for {} s\r".format(0, sign*_vy, 0, dt))
motion.drive(0, sign*_vy, 0)
time.sleep(dt)
motion.stop()
time.sleep(0.5)
if abs(thetahat) > tolerance*100:
# since it's periodic...
# thetahat = round(thetahat%(360) ,2)
dt = abs(thetahat / _w)
sign = -1 if thetahat > 0 else 1
print("motion.drive({},{},{}) for {} s\r".format(0, 0, sign*_w, dt))
motion.drive(0, 0, sign*_w)
time.sleep(dt)
motion.stop()
time.sleep(0.5)
def main():
global _motion_timer
_motion_timer = RepeatedTimer(_motion_timer_period, _handle_motion_timer)
_motion_timer.start()
global _odom_timer
_odom_timer = RepeatedTimer(_odom_timer_period, _handle_odom_timer)
_odom_timer.start()
global _ctrl_timer
_ctrl_timer = RepeatedTimer(_ctrl_timer_period, _handle_ctrl_timer)
_ctrl_timer.start()
use_rcv3 = os.environ['USE_RCV3'] == 'true'
w.init(use_rcv3=use_rcv3)
# TODO: Okay, so Controller.init(None) will automagically load in some PID
# values for each of the x, y, theta position controllers. However, since
# `teleop` is run in real life on different robots, really it should read
# the correct YAML robot config file and load in the PID constants and
# pass them into Controller.init(gains) as in the controller_node.py.
# Two ways to do this: (1) do like os.environ['USE_RCV3'], where the odroid_setup.bash
# script loads up the PID constants as environment variables (this seems messy).
# (2) find a python YAML reading library and just read the file directly from here
# based on which robot this is (you can use os.environ['ROBOT'])
Controller.init()
print 'Started.'
global _velocities, _set_speed, _smooth, _odom_on, _previous_action, _ctrl_on
while(1):
action = get_action()
if action == 'UP':
_set_speed = True
_velocities = (0, _vy, 0)
elif action == 'DOWN':
_set_speed = True
_velocities = (0, -_vy, 0)
elif action == 'RIGHT':
_set_speed = True
_velocities = (_vx, 0, 0)
elif action == 'LEFT':
_set_speed = True
_velocities = (-_vx, 0, 0)
elif action == 'SPIN_CW':
_set_speed = True
_velocities = (0, 0, _w)
elif action == 'SPIN_CCW':
_set_speed = True
_velocities = (0, 0, -_w)
elif action == 'SET_HOME':
Odometry.init()
elif action == 'GO_HOME':
_motion_timer.stop()
motion.stop()
time.sleep(1)
_go_home()
time.sleep(1)
_set_speed = True
_velocities = (0, 0, 0)
_motion_timer.start()
elif action == 'GO_TO_POINT':
_toggle_timers(False)
motion.stop()
time.sleep(1)
_ask_for_point()
time.sleep(1)
_ctrl_on = True
_odom_on = True
_toggle_timers(True)
elif action == 'TOGGLE_CNTRL':
_ctrl_on = not _ctrl_on
print("Controller: {}".format(_ctrl_on))
elif action == 'TOGGLE_SMOOTH':
_smooth = not _smooth
print("Smooth: {}".format(_smooth))
elif action == 'TOGGLE_ODOM':
_odom_on = not _odom_on
print("Odom: {}".format(_odom_on))
elif action == 'BATTERY':
print("\n\r*** Battery: {} ***\n\r".format(get_battery()))
elif action == 'BREAKPOINT':
_toggle_timers(False)
import ipdb; ipdb.set_trace()
_toggle_timers(True)
elif action == 'CALIBRATION_MODE':
_toggle_timers(False)
_deal_with_calibration()
time.sleep(1)
_toggle_timers(True)
elif action == 'DIE':
_toggle_timers(False)
motion.stop()
w.kill()
return sys.exit(0)
else:
_set_speed = True
_velocities = (0, 0, 0)
# handle stopping before changing directions
if _action_requires_stop(action):
_set_speed = False
motion.stop()
time.sleep(0.4)
_set_speed = True
_previous_action = action
print "{}\r".format(_velocities)
def test_calibration(velocity=0.6,
sleep_time=1.5,
M1QPPS=None,
M2QPPS=None,
M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.UpdateVelocityPID(w.M1, q=_m1qpps)
w.UpdateVelocityPID(w.M2, q=_m2qpps)
w.UpdateVelocityPID(w.M3, q=_m3qpps)
_print_stats()
print("velocity: {}\r".format(velocity))
# Forward
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Backward
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left back
motion.drive(velocity * np.cos(2 * np.pi / 3),
velocity * np.sin(2 * np.pi / 3), 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right forward
motion.drive(-np.cos(2 * np.pi / 3) * velocity,
-np.sin(2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right back
motion.drive(
np.cos(-2 * np.pi / 3) * velocity,
np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right Forward
motion.drive(-np.cos(-2 * np.pi / 3) * velocity,
-np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
def move(argument):
global number_of_qr
global number_of_boxes
global box_coordinates
global qr_coordinates
global t
global time0
#global nodes
#update variables required
print('bot: (move called again) right wall dist=',
us.dist(us.right_trig, us.right_echo))
# if right is open, it will stop, turn right
if check('right'):
print('bot:right is open ')
motion.stop()
motion.turn('right')
#move forward
if callable(motion.forward):
thread_forward('right')
if not check('front'):
#if front is blocked, it will check for boxes
motion.stop()
if check_for_qr():
number_of_qr += 1
#if we are still in the first loop num will be <2
if number_of_qr == 1:
#if we have not detected all boxes in the first loop
#it will continue mapping 1st loop
t += time.time() - time0
qr_coordinates.append(t)
motion.turn('left')
time0 = time.time()
#if we have detected all boxes, move to next loop
#we have finished mapping 1st loop
elif number_of_qr == 2:
wait_for_path()
time0 = time.time()
#second loop
elif number_of_qr == 3:
t += time.time() - time0
qr_coordinates.append(t)
wait_for_path()
time0 = time.time()
elif vision.check_for_block():
t += time.time() - time0
box_coordinates.append(t)
wait_for_path()
time0 = time.time()
#if nothing found it will turn left and check the ground
else:
motion.turn('left')
if vision.ground_is_white():
t += time.time() - time0
if number_of_qr >= 2:
end_coordinates.append(t)
#nodes.append('end')
else:
start_coordinates.append(t)
#now we have to go to the qr block...
#to move to the next loop
#set t=0 for next loop
if qr_coordinates[0] > start[1] - qr_coordinates[0]:
wet_run.uturn()
if wet_run.move_till('qr', 'left'):
t = 0
else:
if wet_run.move_till('qr', 'right'):
t = 0
#elif callable(motion.forward):
#put condition for finishing mapping here
if (len(qr_coordinates) == 2) + (number_of_boxes
== 3) + (len(end_coordinates) == 1) < 3:
return move()
else:
return [
qr_coordinates, box_coordinates, start_coordinates, end_coordinates
]
import motion import time import particles for i in range(4): motion.fwd_amt(50) motion.stop() motion.turn(90, "r") motion.stop() ''' particles.initialise() particles.draw() time.sleep(1) particles.update_forward(100) particles.draw() time.sleep(1) particles.update_forward(100) particles.draw() time.sleep(1) particles.update_forward(100) particles.draw() time.sleep(1) particles.update_rotate(90) particles.draw() time.sleep(1)
def spin():
motion.drive(0, 0, 2 * np.pi)
time.sleep(3)
motion.stop()
def spin(): motion.drive(0, 0, 2*np.pi) time.sleep(3) motion.stop()
def test_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.UpdateVelocityPID(w.M1, q=_m1qpps)
w.UpdateVelocityPID(w.M2, q=_m2qpps)
w.UpdateVelocityPID(w.M3, q=_m3qpps)
_print_stats()
print ("velocity: {}\r".format(velocity))
# Forward
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Backward
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left back
motion.drive(velocity * np.cos(2 * np.pi / 3), velocity * np.sin(2 * np.pi / 3), 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right forward
motion.drive(-np.cos(2 * np.pi / 3) * velocity, -np.sin(2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right back
motion.drive(np.cos(-2 * np.pi / 3) * velocity, np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right Forward
motion.drive(-np.cos(-2 * np.pi / 3) * velocity, -np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()