def main():
global client
client = TCPClient(server_ip, server_port, stateChanged=onStateChanged)
print("Client starting")
try:
while True:
rc = client.connect()
sleep(0.01)
if rc:
isConnected = True
while isConnected:
sleep(0.001)
else:
print("Client:-- Connection failed")
sleep(0.1)
except KeyboardInterrupt:
pass
# missin done; close connection
client.disconnect()
threading.cleanup_stop_thread() # needed if we want to restart the client
# EchoClient.py
from easygui import msgbox, enterbox
from tcpcom import TCPClient
import time
def onStateChanged(state, msg):
if state == TCPClient.MESSAGE:
print msg
title = "Echo Client"
port = 5000
host = enterbox("Echo Server IP Address?", title, "localhost", True)
if host != None:
client = TCPClient(host, port, stateChanged=onStateChanged)
client.connect()
for i in range(100):
client.sendMessage(str(i))
time.sleep(0.1)
client.disconnect()
class DeliverAIBot():
def __init__(self, map, init_office):
self.my_map = map # Map for this robot
self.position = init_office # Starting position
self.coords = self.position.coords # Current coordinates of the robot
# Motors which are attached to the EV3
self.motors = [
ev3.LargeMotor('outB'),
ev3.LargeMotor('outA'),
ev3.LargeMotor('outD'),
ev3.LargeMotor('outC'),
]
# Which way are we facing
self.bearing = 0
self.tape_side = "right"
self.cs = ev3.ColorSensor("in1") # Colour sensor for junction detection # noqa: E501
self.rs = ev3.ColorSensor("in2") # Reflectance sensor for line-following # noqa: E501
self.rs.MODE_COL_REFLECT
self.cs.MODE_COL_COLOR
# Add Config Parsing
self.config = configparser.ConfigParser()
self.config.read('/home/robot/config.txt')
self.web_server_ip = self.config['DELIVERAI']['WEB_SERVER_IP']
self.web_server_port = int(self.config['DELIVERAI']['WEB_SERVER_PORT'])
# Add Client Connection
self.client_connection = TCPClient(
self.config['DELIVERAI']['PI_IP'],
server_port,
stateChanged=self.onMsgRecv,
isVerbose=False
)
self.connected = False
self.try_connect()
# Set up threading for non blocking line following
self.movment_thread = None
self.stop_event = threading.Event()
self.stop_event.clear()
self.stop_it_pls = False
# Set up Alarm variables
self.alarm_active = False
self.alarm_on = False
self.mode_debug_on = False
# Import offices
self.download_json_map()
def __del__(self):
# Disconnect from the server, clean up the threads then we can exit
self.client_connection.disconnect()
threading.cleanup_stop_thread()
print("[__del__] CleanedUp - Disconnected from Server")
def download_json_map(self):
json_raw = urllib.request.urlopen(
"http://{}:{}/api/map.json".format(
self.web_server_ip,
self.web_server_port
)
)
self.my_map.addJsonFile(json_raw.read().decode())
print("[download_json_map] Updated Map - Added " + str(len(self.my_map.offices))) # noqa: E501
def goTo(self, destination):
''' Travel to the destination from the current position '''
route = self.route(destination) # List of coordinates to pass through
print("Route: ", route)
if len(route) == 1:
print("Already here!")
return
for i in range(0, len(route)-1):
# Get new bearing to travel on
self.bearing = self.getBearing(route[i], route[i+1])
self.update_server()
self.send_bearing(self.bearing)
# Update colour/reflectance sensors & Travel to the next stop on
# the route
self.updateColourSensors()
bonvoyage = self.followLine(self.bearing)
if bonvoyage == "success":
# If successful we update our position
self.position = self.my_map.offices[route[i+1]]
self.coords = self.position.coords
self.update_server()
elif bonvoyage == "obstacle":
print("REROUTING....")
self.rerouteMe(destination, self.my_map.offices[route[i+1]])
return
else:
print("UNKNOWN ERROR.")
return
time.sleep(0.1)
print("Arrived at " + self.position.name + "\'s office.")
self.send_arrived()
def updateColourSensors(self):
''' Update colour/reflectance sensor depending on the bearing
so colour sensor is in front '''
if (
self.bearing == 0 or
(self.bearing == 90 and self.tape_side == "right") or
(self.bearing == 270 and self.tape_side == "left")
):
cs_port = "in1"
else:
cs_port = "in2"
rs_port = "in2" if cs_port == "in1" else "in1"
self.cs = ev3.ColorSensor(cs_port)
assert self.cs.connected
self.rs = ev3.ColorSensor(rs_port)
assert self.rs.connected
self.cs.mode = 'COL-COLOR'
self.rs.mode = 'COL-REFLECT'
def rerouteMe(self, destination, next_stop):
''' Find a different route to the destination '''
# Notify admin of issue
self.notify_server_of_obs(next_stop.coords[0], next_stop.coords[1])
# Reverse until we hit a junction
self.bearing = (self.bearing + 180) % 360
self.update_server() # Probably don't need this to be honest
self.send_bearing(self.bearing) # Or this
self.stop_event.clear()
self.stop_it_pls = False
self.tape_side = "right" if self.tape_side == "left" else "left"
self.updateColourSensors()
self.followLine(self.bearing)
# Delete edge
temp = self.position
side = self.delPath(self.position, next_stop)
# Call goTo to destination
self.goTo(destination)
# Add edge back
self.addPath(temp, next_stop, side)
def delPath(self, o1, o2):
''' Delete an edge between the two given offices '''
# Find which neighbour we want
side = [k for (k, v) in o1.getNeighbours().items() if v == o2][0]
# Remove the two edges in question
if side == "right":
o1.setRightNeighbour(None)
o2.setLeftNeighbour(None)
elif side == "left":
o1.setLeftNeighbour(None)
o2.setRightNeighbour(None)
elif side == "upper":
o1.setUpperNeighbour(None)
o2.setLowerNeighbour(None)
elif side == "lower":
o1.setLowerNeighbour(None)
o2.setUpperNeighbour(None)
else:
print("ERROR. Offices are not neighbours")
return
# Return the side so we can add the edge back later
# (when carrying out a reroute)
return side
def addPath(self, o1, o2, side):
''' Add an edge between the two given offices '''
if side == "right":
o1.setRightNeighbour(o2)
o2.setLeftNeighbour(o1)
elif side == "left":
o1.setLeftNeighbour(o2)
o2.setRightNeighbour(o1)
elif side == "upper":
o1.setUpperNeighbour(o2)
o2.setLowerNeighbour(o1)
elif side == "lower":
o1.setLowerNeighbour(o2)
o2.setUpperNeighbour(o1)
else:
print("ERROR. Invalid side.")
return
def goToCoord(self, coord=(0, 0)):
''' Travel to the given coords from the current position'''
destination = self.my_map.offices[coord]
print("Office at (" + str(coord[0]) + "," + str(coord[1]) + ") is " + destination.name) # noqa: E501
print("Heading to " + destination.name + "\'s office at (" + str(coord[0]) + "," + str(coord[1]) + ").") # noqa: E501
self.goTo(destination)
def getBearing(self, a, b):
''' Return the bearing (cardinal direction) to move in to
get from a to b '''
# Find the new bearing to travel on
if a[0] == b[0]:
if b[1] > a[1]:
new_bearing = 90
else:
new_bearing = 270
else:
if b[0] > a[0]:
new_bearing = 0
else:
new_bearing = 180
# Update which side the black line will be on in relation to the robot
diff = (self.bearing - new_bearing) % 360
if self.tape_side == "right":
if diff == 180 or diff == 90:
self.tape_side = "left"
else:
if diff == 180 or diff == 270:
self.tape_side = "right"
return new_bearing
def followLine(self, bearing):
# Reflectance value we aim to maintain by following the black line
target = 20
# Motors to move forward on this bearing
fmotors = self.whichMotors(bearing)
# We begin on a junction, we must move off it
while self.cs.color == 5:
self.moveMotor(fmotors[0], speed=-300, duration=500)
self.moveMotor(fmotors[1], 300, 500)
# Line-following
while True:
# If we have reached a junction we must stop
if self.cs.color == 5:
print("Reached an office! Stopping...")
self.stopMotors()
return "success"
error = target - self.rs.value() # Read the reflectance sensor
count = 0
# Obstacle-detection
while self.stop_event.is_set():
print("Stopping. Obstacle in the way.")
self.stopMotors()
count += 1
if count > 10: # If we have been stuck for >10 seconds reroute
print("Rerouting...")
self.stop_event.clear()
return "obstacle"
# Wait one second before rechecking the obstacle has moved
time.sleep(1)
# Continue with regular line-following
if error > 2: # Too far on black
self.correctBlack(fmotors)
continue
elif error < -2: # Too far on white
self.correctWhite(fmotors)
continue
# Otherwise just move forward
self.moveMotor(fmotors[0], speed=-300, duration=500)
self.moveMotor(fmotors[1], 300, 500)
def whichMotors(self, bearing):
''' Return (left,right) motors to turn when moving on bearing '''
if bearing == 0:
return (0, 2)
elif bearing == 90:
return (3, 1)
elif bearing == 180:
return (2, 0)
elif bearing == 270:
return (1, 3)
else:
print("ERROR. Illegal bearing.")
return None
def correctBlack(self, motors):
''' Correct the robot's position if we are on black '''
self.translate(motors, "black") # Translation
# Then rotation
if self.tape_side == "right":
self.moveMotor(motors[0], speed=-240, duration=500)
self.moveMotor(motors[1], 300, 500)
else:
self.moveMotor(motors[0], speed=-300, duration=500)
self.moveMotor(motors[1], 240, 500)
def correctWhite(self, motors):
''' Correct the robot's position if we are on white '''
self.translate(motors, "white") # Translation
# Then rotation
if self.tape_side == "right":
self.moveMotor(motors[0], speed=-300, duration=500)
self.moveMotor(motors[1], 240, 500)
else:
self.moveMotor(motors[0], speed=-240, duration=500)
self.moveMotor(motors[1], 300, 500)
def translate(self, fmotors, colour):
''' Translate the robot to correct its bearing while line-following '''
motors = self.whichMotors((self.bearing+90)%360) if colour == "black" else self.whichMotors((self.bearing-90)%360) # Choose the side motors to move # noqa E501
if self.tape_side == "right":
self.moveMotor(motors[0], speed=-100, duration=75)
self.moveMotor(motors[1], 100, 75)
else:
self.moveMotor(motors[0], speed=100, duration=75)
self.moveMotor(motors[1], -100, 75)
def moveMotor(self, motor=0, speed=100, duration=500, wait=False):
''' Move a specific motor (given speed and duration of movement) '''
motor = self.motors[motor]
assert motor.connected
motor.run_timed(speed_sp=speed, time_sp=duration)
def stopMotors(self):
for motor in self.motors:
motor.stop()
def route(self, destination):
''' Search the map to find the shortest route to the destination '''
return self.findShortestRoute(
self.my_map.neighbourDict(),
self.coords,
destination.coords
)
def findShortestRoute(self, nbours, start, end, route=[]):
''' Find the shortest route from start to end '''
route = route + [start]
if start == end:
return route
if start not in nbours.keys():
return None
shortest = None
for nbour in nbours[start]:
if nbour not in route:
newroute = self.findShortestRoute(nbours, nbour, end, route)
if newroute:
if not shortest or len(newroute) < len(shortest):
shortest = newroute
return shortest
def notify_server_of_obs(self, x, y):
to_access = "http://" + self.web_server_ip
to_access = to_access + ":" + str(self.web_server_port)
to_access = to_access + "/api/botinfo"
to_provide = {
"name": robot_name,
"x_loc": self.coords[0],
"y_loc": self.coords[1],
"x_issue": x,
"y_issue": y,
"state": "OBSTACLEINWAY",
"battery_volts": ev3.PowerSupply().measured_volts,
"bearing": self.send_bearing(self.bearing)
}
post_data = urllib.parse.urlencode(to_provide)
try:
urllib.request.urlopen(
url='{}?{}'.format(to_access, post_data),
data="TEMP=TEMP".encode()
)
except: # noqa: E722
print("[notify_server_of_obs] Has failed - failed to connect to: " + to_access) # noqa: E501
def update_server(self):
to_access = "http://" + self.web_server_ip
to_access = to_access + ":" + str(self.web_server_port)
to_access = to_access + "/api/botinfo"
to_provide = {
"name": robot_name,
"x_loc": self.coords[0],
"y_loc": self.coords[1],
"state": "TEMP",
"battery_volts": ev3.PowerSupply().measured_volts,
"bearing": self.send_bearing(self.bearing)
}
post_data = urllib.parse.urlencode(to_provide)
try:
urllib.request.urlopen(
url='{}?{}'.format(to_access, post_data),
data="TEMP=TEMP".encode()
)
except: # noqa: E722
print("[update_server] Has failed - failed to connect to: " + to_access) # noqa: E501
def try_connect(self):
connection_attempts = 0
while (not(self.connected)):
connection = self.client_connection.connect()
if connection:
self.connected = True
self.update_server()
ev3.Sound().speak("Ready to Deliver")
else:
self.connected = False
connection_attempts += 1
if connection_attempts > 20:
print("[try_connect] Tried 20 times - STOPPING!")
return
time.sleep(1)
def onMsgRecv(self, state, msg):
if (state == "CONNECTED"):
self.connected = True
elif (state == "DISCONNECTED"):
self.connected = False
print("[onMsgRecv] ERROR: SERVER HAS DISCONNECTED")
self.try_connect()
elif (state == "MESSAGE"):
self.process_msg(msg)
def process_msg(self, msg):
broken_msg = msg.split("$")
print("process start")
if (self.mode_debug_on):
self.process_debug_msg(msg)
elif (broken_msg[0] == "GOTO"):
# As soon as GOTO is received robot starts to go there
print(broken_msg[1] + " " + broken_msg[2])
input = (int(broken_msg[1]), int(broken_msg[2]))
inputer = {'coord': input}
print("[process_msg] Moving to ")
self.movment_thread = threading.Thread(
target=self.goToCoord,
kwargs=inputer
).start()
elif (broken_msg[0] == "STOP"):
# STOP stopes the robot ASAP is moving, if not does not allow to go
# Sets the event stop_event that can be seen by other threads
self.status = "STOPPED"
self.stop_event.set()
self.stop_it_pls = True
# print("[process_msg] STOP EVENT MSG RECEIVED")
elif (broken_msg[0] == "CONT"):
# Start moving again
self.status = "MOVING"
self.stop_event.clear()
self.stop_it_pls = False
print("CONTINUE")
elif (broken_msg[0] == "STATUS"):
# Get the current status that the robot thinks it is in
print("[process_msg] status requested")
self.client_connection.sendMessage(self.status)
elif (broken_msg[0] == "GETLOC"):
# Get the current co-ordinates of the robot
print("[process_msg] location requested")
self.client_connection.sendMessage(
self.coords[0] + "$" + self.coords[1]
)
elif (broken_msg[0] == "ALARM"):
# Set the alarm off
self.alarm_active = True
if (not(self.alarm_on)):
threading.Thread(target=self.alarm).start()
self.alarm_on = True
elif (broken_msg[0] == "ALARMSTOP"):
# Stop the alarm
self.alarm_active = False
self.alarm_on = False
print("[process_msg] Stopping Alarm - cleared by admin")
elif (broken_msg[0] == "DEBUGMODEON"):
self.mode_debug_on = True
elif (broken_msg[0] == "UPDATEMAP"):
self.my_map = Map()
self.download_json_map()
elif (broken_msg[0] == "POWEROFF"):
os.system("sudo poweroff")
elif (broken_msg[0] == "AUTOCLOSE"):
self.auto_close_announce()
else:
print("[process_msg] UNPROCESSED MSG received: " + msg)
def auto_close_announce(self):
ev3.Sound.speak("ALERT, ALERT, the door is about the close!")
time.sleep(5)
ev3.Sound.beep("-f 350 -d 10 -r 10")
def process_debug_msg(self, msg):
if (msg == "DEBUGMODEOFF"):
self.mode_debug_on = False
f = open("cmd_recved.txt", "a+")
f.write(msg)
def send_arrived(self):
self.client_connection.sendMessage("ARRIVED")
def send_bearing(self, bearing):
dir_go = -1
if (bearing == 0):
dir_go = 0
elif (bearing == 90):
dir_go = 1
elif (bearing == 180):
dir_go = 2
elif (bearing == 270):
dir_go = 3
return dir_go
def alarm(self):
print("[alarm] Alarming!")
while (self.alarm_active):
beep_args = "-r 30 -l 100 -f 1000"
# Starts a new thread as the EV3's beep blocks the sleep and
# disarming of the alarm
threading.Thread(
target=ev3.Sound().beep,
args=(beep_args,)
).start()
time.sleep(6)
print("[alarm] Disarmed - Stopped Alarming")
return
class Toddler:
__version = '2018a'
def __init__(self, IO):
print('[Toddler] I am toddler {} playing in a sandbox'.format(
Toddler.__version))
# Start up all the External IO stuff
self.camera = IO.camera.initCamera('pi', 'low')
self.getInputs = IO.interface_kit.getInputs
self.getSensors = IO.interface_kit.getSensors
self.mc = IO.motor_control
self.sc = IO.servo_control
self.motor_control = MotorMover()
# Get the config from the file
self.config = ConfigParser.ConfigParser()
self.config.read('/home/student/config.txt')
# Set up servers and client
self.server_port = 5010
self.client_port = int(
self.config.get(
"DELIVERAI",
'WEB_SERVER_COMM_PORT'
))
self.client_connect_address = self.config.get(
"DELIVERAI",
'WEB_SERVER_IP'
)
self.server = TCPServer(
self.server_port,
stateChanged=self.on_server_msg,
isVerbose=False
)
self.client = TCPClient(
self.client_connect_address,
self.client_port,
stateChanged=self.on_client_msg
)
self.connected = False
self.try_connect()
# Set up robot state
self.not_sent_stop = [True, True, True, True]
self.mode = "READY"
self.pick_from = (0, 0)
self.deliver_to = (0, 0)
self.current_movment_bearing = 0
self.alarm = False
self.box_open = False
self.mode_debug_on = False
self.box_timeout = None
self.sleep_time = 0.05
# Set up sensor + motor values
self.accel = LSM303()
self.acc_counter = [0, 0, 0]
self.calibrated = 0
self.base_accel = np.zeros(3) # will be set during calibration
self.accel_val_num = 20
self.last_accels = deque()
self.accel_data = deque(
[-1] * self.accel_val_num
) # accelerometer array used for smoothing
self.vel = np.zeros(3)
self.pos = np.zeros(3)
self.last_accel = np.zeros(3)
self.last_vel = np.zeros(3)
self.door_mech_motor = 2
self.lock_motor = 3
self.motor_speed = 40
def __del__(self):
print("[__del__] Cleaning Up...")
self.client.disconnect()
threading.cleanup_stop_thread()
# CONTROL THREAD
def control(self):
self.detect_obstacle()
self.accel_alarm() # needs improvement to be used again
self.box_alarm()
time.sleep(self.sleep_time)
# VISION THREAD
def vision(self):
# Block vision branch for now because we don't use it
time.sleep(0.05)
def open_box_motor(self):
self.open_lock()
self.motor_control.motor_move(self.door_mech_motor, self.motor_speed)
# Open lid until bump switched is pressed
while (self.getInputs()[1] == 1):
time.sleep(0.01)
self.mc.stopMotor(self.door_mech_motor)
# If the close command does not get sent after 60 seconds the box will
# close its self
self.box_timeout = threading.Timer(60.0, self.auto_close_box)
self.box_timeout.start()
def auto_close_box(self):
self.server.sendMessage("AUTOCLOSE")
time.sleep(10)
self.close_box_motor()
def close_box_motor(self):
self.motor_control.motor_move(self.door_mech_motor, -self.motor_speed)
# Close lid until bump switched is pressed
while (self.getInputs()[2] == 1):
time.sleep(0.01)
self.mc.stopMotor(self.door_mech_motor)
self.close_lock()
time.sleep(1)
if (self.state == "PICKINGUP"):
self.state = "DELIVERING"
self.send_robot_to(self.deliver_to[0], self.deliver_to[1])
if (self.state == "DELIVERING"):
self.state = "READY"
self.client.sendMessage("READY")
def open_lock(self):
self.box_open = True
self.mc.stopMotor(self.lock_motor)
def close_lock(self):
self.mc.setMotor(self.lock_motor, 100)
self.box_open = False
def on_server_msg(self, state, msg):
if state == "LISTENING":
print("[on_server_msg] Server:-- Listening...")
elif state == "CONNECTED":
print("[on_server_msg] Server:-- Connected to" + msg)
elif state == "MESSAGE":
print("[on_server_msg] Server:-- Message received:" + msg)
self.process_server_message(msg)
self.server.sendMessage("OK")
def on_client_msg(self, state, msg):
if (state == "CONNECTED"):
print("[on_client_msg] Sucess - Connected to Server :" + msg)
elif (state == "DISCONNECTED"):
print(
"[on_client_msg] Disconnected from Server - Trying to "
"connect again...")
self.try_connect()
elif (state == "MESSAGE"):
print("[on_client_msg] Message Received from server")
self.process_client_message(msg)
def try_connect(self):
while not (self.connected):
conn = self.client.connect()
if conn:
self.connected = True
else:
self.connected = False
time.sleep(10)
def process_server_message(self, msg):
print("[process_server_message] Processing Msg Received")
broken_msg = msg.split("$")
if (broken_msg[0] == "ARRIVED"):
print("[process_server_message] Arrived at dest requested")
if (self.state == "GOINGHOME"):
print("[process_server_message] At home awaiting command...")
# do nothing
elif (self.state == "PICKINGUP" or self.state == "DELIVERING"):
print("[process_server_message] Waiting for authorisation")
self.request_authentication()
elif (broken_msg[0] == "BEARING"):
print("bearing recved " + broken_msg[1])
self.current_movment_bearing = int(broken_msg[1])
def process_client_message(self, msg):
print("[process_client_message] Processing Msg Recived")
broken_msg = msg.split("$")
if (self.mode_debug_on):
print("[process_client_message] Debug Msg Received")
self.process_debug_msg(msg)
elif (broken_msg[0] == "GOHOME"):
self.state = "GOINGHOME"
self.send_robot_to(0, 0)
elif (broken_msg[0] == "PICKUP"):
self.state = "PICKINGUP"
self.pick_from = (int(broken_msg[1]), int(broken_msg[2]))
self.deliver_to = (int(broken_msg[4]), int(broken_msg[5]))
self.send_robot_to(self.pick_from[0], self.pick_from[1])
elif (broken_msg[0] == "OPEN"):
self.open_box_motor()
elif (broken_msg[0] == "CLOSE"):
self.box_timeout.cancel() # Cancel the auto-close
self.close_box_motor()
elif (broken_msg[0] == "ALARMSTOP"):
self.server.sendMessage("ALARMSTOP")
self.alarm = False
elif (broken_msg[0] == "DEBUGMODEON"):
self.mode_debug_on = True
elif (broken_msg[0] == "TESTCONNEV3"):
self.test_conn_ev3()
elif (broken_msg[0] == "UPDATEMAP"):
self.server.sendMessage("UPDATEMAP")
elif (broken_msg[0] == "BEARING"):
self.current_movment_bearing = int(broken_msg[1])
elif (broken_msg[0] == "POWEROFF"):
self.server.sendMessage("POWEROFF")
self.client.disconnect()
time.sleep(5)
os.system("sudo poweroff")
def process_debug_msg(self, msg):
if (msg == "DEBUGMODEOFF"):
self.mode_debug_on = False
f = open("cmd_recved.txt", "a+")
f.write(msg + "\n")
def send_robot_to(self, x, y):
print("[send_robot_to] Sending Robot to " + str(x) + ", " + str(y))
self.server.sendMessage("GOTO$" + str(x) + "$" + str(y))
def request_authentication(self):
self.client.sendMessage("AUTHENTICATE")
# Detect obstacle within the range of min_dist
def detect_obstacle(self):
min_dist = 20
an_input = self.getSensors()
ir_input = [an_input[i] for i in range(4)]
dist = [self.input_to_cm(x) for x in ir_input]
for i in range(4):
if dist[i] != -1 and dist[i] < min_dist:
self.stop(i)
else:
self.continue_path(i)
# Function for reacting according to obstacle in direction dir where dir=
# 0 - Front, 1 - Right, 2 - Back, 3 - Left
def stop(self, dir):
# If the dirrection is NOT (current_movment_bearing + 2 mod 4) i.e.
# behind us - we can send stop
if (dir != ((self.current_movment_bearing + 2) % 4)):
# print("[stop] Obstacle in {} - currently facing {}".format(
# dir, self.current_movment_bearing)) # noqa: E501
self.not_sent_stop[dir] = False
self.server.sendMessage("STOP")
def continue_path(self, i):
if not (self.not_sent_stop[i]):
print("[continue_path] Obstacle in %d cleared!" % i)
self.not_sent_stop[i] = True
if (reduce(operator.and_, self.not_sent_stop, True)):
self.server.sendMessage("CONT")
# Convert analog input from IR sensor to cm
# Formula taken from:
# https://www.phidgets.com/?tier=3&catid=5&pcid=3
# &prodid=70#Voltage_Ratio_Input
# IR sensor model used: GD2D12 - range: 10-80cm
def input_to_cm(self, an_input):
# Input has to be adapted as the input differs from the value range on
# the website by a factor of 1000
voltageRatio = an_input / 1000.0
if voltageRatio > 0.08 and voltageRatio < 0.53: # from the website
dist = 4.8 / (voltageRatio - 0.02)
else:
dist = -1
return dist
# Checks for unauthorized opening of the box - using bump sensor
def box_alarm(self):
box_alarm = self.getInputs()[2] # Get sensor value from bump switch
if box_alarm == 1 and not (self.box_open):
self.send_alarm()
print("[box_alarm] Box Open - Not Due to be")
def send_alarm(self):
self.server.sendMessage("ALARM")
def integrate_accel(self, accel, last_accel):
last_accel = np.array(last_accel)
delta_vel = (accel + ((accel - last_accel) / 2.0)) * self.sleep_time
self.vel = np.array(delta_vel) + self.vel
# Check if no accel in any direction and increase corresponding counter
for i in range(len(accel)):
if accel[i] == 0:
self.acc_counter[i] += 1
else:
self.acc_counter[i] = 0
# If the last 25 values for 1 direction have been 0, set velocity to 0
for i in range(len(self.acc_counter)):
if self.acc_counter[i] > 15:
self.vel[i] = 0
return np.array(self.vel)
def integrate_vel(self, vel, last_vel):
vel = np.array(vel)
last_vel = np.array(last_vel)
delta_pos = (vel + ((vel - last_vel) / 2.0)) * self.sleep_time
self.pos = np.array(delta_pos + self.pos)
return np.array(self.pos)
# Checks for unexpected movement/robot being stolen - using accelerometer
def accel_alarm(self):
if self.calibrated < 3:
self.calibrate_accel()
return
cur_accel = self.read_smooth_accel()
# If we haven't had enough readings for averaging - don't continue
if (cur_accel == -1):
return
accel = np.array(cur_accel)
vel = self.integrate_accel(accel, self.last_accel)
pos = self.integrate_vel(vel, self.last_vel)
print("--------------------------------------------------------------")
print("Accel: x:%.2f y:%.2f z:%.2f" % (
accel[0], accel[1], accel[2]))
print("Vel: x:%.2f y:%.2f z:%.2f" % (vel[0], vel[1], vel[2]))
print("Pos: x:%.2f y:%.2f z:%.2f" % (pos[0], pos[1], pos[2]))
if abs(vel[2]) > 6 or abs(vel[1]) > 10 or abs(vel[0]) > 10:
if not (self.alarm):
self.send_alarm()
self.alarm = True
print("[accel_alarm] ALARM STATE -- X: %.2f -- Y: %.2f -- Z: %.2f " % (vel[0], vel[1], vel[2])) # noqa E501
self.last_accel = accel
self.last_vel = vel
# Smooth accelerometer output by taking the average of the last n values
# where n = len(self.accel_data)
def read_smooth_accel(self):
# print(self.base_accel)
cur_accel, _ = self.accel.read()
# calibrate input
cur_accel = (np.array(cur_accel) - self.base_accel).tolist()
# To filter out mechanical noise
for i in range(len(cur_accel)):
if cur_accel[i] > -30 and cur_accel[i] < 30:
cur_accel[i] = 0
self.accel_data.pop()
self.accel_data.appendleft(cur_accel)
# For the first len(accel_data) values the average is not
# representative - return -1 to represent that
if -1 in self.accel_data:
return -1
av_accel = [sum(i) / float(len(i)) for i in zip(*self.accel_data)]
for i in range(len(av_accel)):
if av_accel[i] < 20 and av_accel[i] > -20:
av_accel[i] = 0
return av_accel
def calibrate_accel(self):
cur_accel, _ = self.accel.read()
if -1 in self.accel_data:
self.accel_data.pop()
self.accel_data.appendleft(cur_accel)
else:
av_accel = [sum(i) / float(len(i)) for i in zip(*self.accel_data)]
self.base_accel = np.array(av_accel)
self.calibrated = self.calibrated + 1
self.accel_data = deque([-1] * self.accel_val_num)
# reset data_accel deque
def test_conn_ev3(self):
self.server.sendMessage("DEBUGMODEON")
time.sleep(2)
file_in = open("cmds_to_send.txt", "r")
lines = file_in.readlines()
for l in lines:
self.server.sendMessage(l)
