class Motob:
"""
The motor object (motob) manifests an interface between a behavior and one or more motors (a.k.a. actuators).
It contains (at least) the following instance variables:
1. motors - a list of the motors whose settings will be determined by the motob.
2. value - a holder of the most recent motor recommendation sent to the motob.
Its primary methods are:
1. update - receive a new motor recommendation, load it into the value slot, and operationalize it.
2. operationalize - convert a motor recommendation into one or more motor settings, which are sent to
the corresponding motor(s)
"""
def __init__(self):
self.motor = Motors()
self.value = None
def update(self, recommendation):
self.value = recommendation
self.operationalize()
def operationalize(self):
"""
convert motor recommendation into motor setting
send to motor
format value: [direction, speed, duration]
:return:
"""
for element in self.value:
if element[0] == "f":
self.motor.forward(element[1], element[2])
elif element[0] == "b":
self.motor.backward(element[1], element[2])
elif element[0] == "l":
self.motor.left(element[1], element[2])
elif element[0] == "r":
self.motor.right(element[1], element[2])
def crashTest():
ZumoButton().wait_for_press()
sensor = Crash_sensor()
motor = Motors()
counter = 0
f_value = sensor.calculateFront()
ir_command = sensor.calculateSides()
while True:
counter += 1
if counter >= 5:
f_value = sensor.calculateFront()
ir_command = sensor.calculateSides()
counter = 0
if ir_command == "LEFT":
motor.left(0.5, 0.1)
elif ir_command == "RIGHT":
motor.right(0.5, 0.1)
elif ir_command == "BACKWARD":
motor.backward(0.3, 0.5)
else:
motor.forward(0.3, 0.01)
if f_value == 1000:
motor.stop()
break
class Motob():
"""Motor object that manifests an interface between a behavior and motor(s)"""
def __init__(self):
self.motors = Motors()
self.value = "" # nyeste motor anbefaling
def update(
self, recommendation
): # må snakke med arbitrator hvilken form denne parameteren har
"""Mottar ny anbefaling til motoren, legger det i value og gjør det til en operasjon"""
if self.value != recommendation:
self.value = recommendation
self.motors.stop()
self.operationalize()
def operationalize(self):
"""konverterer en anbefaling til en eller flere motor instillinger, som sendes til korresponderende motor(er)"""
if self.value[0] == "F":
self.motors.forward(0.25, 0.6)
elif self.value[0] == "B":
self.motors.backward(0.25, 0.6)
elif self.value[0] == "L":
self.motors.left(
0.5, (int(self.value[1]) * 10 + int(self.value[2])) * 0.01)
elif self.value[0] == "R":
self.motors.right(
0.5, (int(self.value[1]) * 10 + int(self.value[2])) * 0.01)
elif self.value[0] == "T":
self.motors.set_value([-1, 1], 0.5)
elif self.value[0] == "S":
self.motors.stop()
class Motob:
def __init__(self):
self.motors = Motors()
self.value = None
# Updates the motors value and sends direction further
def update_motor(self, value):
self.value = value
print(self.value)
self.operationalize()
# Takes a recommendation and makes the motors do the operation.
def operationalize(self):
recom = self.value
if recom[0] == 'F':
self.motors.forward(speed=recom[1]*0.01, dur=1) # multiply the degrees with 0.01 to get percent for speed
elif recom[0] == 'B':
self.motors.backward(speed=recom[1]*0.01, dur=1)
elif recom[0] == 'L':
self.motors.left(speed=recom[1]*0.01, dur=1)
elif recom[0] == 'R':
self.motors.right(speed=recom[1]*0.01, dur=1)
elif recom[0] == 'S':
self.motors.stop()
# Makes the robot rotate, vector [-1, 1], [L, R].
def rotate(self, vector):
self.motors.set_value(vector)
# Optional stop method, duplicate in operationalize()
def stop(self):
self.motors.stop()
def dancer():
m = Motors()
m.forward(0.2, 3)
m.backward(0.2, 3)
m.right(0.5, 3)
m.left(0.5, 3)
m.backward(0.3, 2.5)
m.set_value([0.5, 0.1], 10)
m.set_value([-0.5, -0.1], 10)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(.2, 3)
m.forward(.3, 2)
m.right(.5, 3)
m.left(.5, 3)
m.backward(.3, 2.5)
m.set_value([.5, .1], 10)
m.set_value([-.5, -.1], 10)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(0.2, 3)
m.backward(0.2, 3)
m.right(0.5, 3)
m.left(0.5, 3)
m.backward(0.3, 2.5)
m.set_value([0.5, 0.1], 10)
m.set_value([-0.5, -0.1], 10)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(.2,3)
m.backward(.2,3)
m.right(.5,3)
m.left(.5,3)
m.backward(.3,2.5)
m.set_value([.5,.1],10)
m.set_value([-.5,-.1],10)
def explorer(dist=10):
ZumoButton().wait_for_press()
m = Motors(); u = Ultrasonic()
while u.update() > dist:
m.forward(.2,0.2)
m.backward(.1,.5)
m.left(.5,3)
m.right(.5,3.5)
sleep(2)
while u.update() < dist*5:
m.backward(.2,0.2)
m.left(.75,5)
def explorer(dist=10):
ZumoButton().wait_for_press()
m = Motors(); u = Ultrasonic()
while u.update() > dist:
m.forward(.2,0.2)
m.backward(.1,.5)
m.left(.5,3)
m.right(.5,3.5)
sleep(2)
while u.update() < dist*5:
m.backward(.2,0.2)
m.left(.75,5)
def explorer(dist=10):
m = Motors()
u = Ultrasonic()
while u.update() > dist:
m.forward(0.2, 0.2)
m.backward(.1, 0.5)
m.left(.5, 3)
m.right(.5, 3.5)
sleep(2)
while u.update() < dist * 5:
m.backward(.2, 0.2)
m.left(.75, 5)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
print("hei")
m.forward(0.2, 3)
m.backward(0.2, 3)
m.right(0.5, 3)
m.left(0.5, 3)
m.backward(0.3, 2.5)
m.set_value([0.5, 0.1], 10)
m.set_value([-0.5, -0.1], 10)
print("hei2")
class Motob:
def __init__(self):
self.motors = Motors()
self.values = []
self.drive_speed = 0.15
self.turn_speed = 0.5
def update(self, recommendation):
self.values.append(recommendation)
self.operationalize()
def operationalize(self):
"""
L: left
R: right
F: forward
B: backward
S: stop
"""
print(self.values)
for rec in self.values:
direction = rec[0]
print(direction)
if len(rec) == 1:
print(direction)
if direction == 'F':
self.motors.set_value([self.drive_speed, self.drive_speed])
elif direction == 'B':
self.motors.set_value(
[-self.drive_speed, -self.drive_speed])
elif direction == 'S':
self.motors.stop()
elif len(rec) == 2:
second_param = (rec[1])
if direction == 'L':
self.motors.left(self.turn_speed,
Motob.degrees_to_time(second_param))
elif direction == 'R':
self.motors.right(self.turn_speed,
Motob.degrees_to_time(second_param))
elif direction == 'F':
self.motors.forward(self.drive_speed, second_param)
elif direction == 'B':
self.motors.backward(self.drive_speed, second_param)
self.values = []
@staticmethod
def degrees_to_time(degrees):
return (degrees / 360) * 6
class motob:
"""Motob klasse har en motor"""
def __init__(self):
"""Create empty list as value"""
self.motor = Motors()
# ("l",45,+0.5)
self.value_drive = ["l", 0, 0] # a list [direction, degrees, speed]
self.value_halt = False # The value of the halt flag
#self.dir_list = ['l','r','f','b']
def update(self, mot_roc):
"""sette en ny value"""
self.value_drive = mot_roc[0]
self.value_halt = mot_roc[1]
def operationalize(self, dur=0.05):
"""apply value, r: Right, l:Left, f:Forward, b:Backward"""
#dur = 3
turn_speed = self.value_drive[1] / 100
if turn_speed > 1:
turn_speed = 1
# cond_left_right = False
# if self.value_drive[1] == 0:
# If we are turning 0 degrees, cond_left_right = True
# cond_left_right = True
#print("I am in loop")
if self.value_drive[
0] == 'l' and self.value_drive[1] > 0 and not self.value_halt:
# turn left and we are turning, but why not call left?
self.motor.set_value((-self.value_drive[2], self.value_drive[2]),
turn_speed) # 10 eller dur
#self.motor.left(self.value_drive[2], dur)
elif self.value_drive[
0] == 'r' and self.value_drive[1] > 0 and not self.value_halt:
# turn right, and we are turning, try motors.right??
self.motor.set_value((self.value_drive[2], -self.value_drive[2]),
turn_speed) # 10 eller dur
#self.motor.right(self.value_drive[2], dur)
elif self.value_drive[0] == 'f' and self.value_drive[
1] == 0 and not self.value_halt:
# Drive forward
self.motor.forward(abs(self.value_drive[2]), dur)
#self.motor.set_value((self.value_drive[2], self.value_drive[2]), self.value_drive[2])
elif self.value_drive[0] == 'b' and self.value_drive[
1] == 0 and not self.value_halt:
# Drive backwards
self.motor.backward(abs(self.value_drive[2]), dur)
#self.motor.set_value((-self.value_drive[2], -self.value_drive[2]), self.value_drive[2])
elif self.value_halt:
# If we are done, the haltflag is True, stop
self.motor.stop()
def test_regul():
"""
Make a square with the robot and at the end make it turn the
other direction (just for fun).
"""
DISTANCE = 50
ANGLE = 90
motors = Motors(5)
motors.forward(DISTANCE)
wait_motors(motors)
motors.turn_left(ANGLE)
wait_motors(motors)
motors.forward(DISTANCE)
wait_motors(motors)
motors.turn_left(ANGLE)
wait_motors(motors)
motors.forward(DISTANCE)
wait_motors(motors)
motors.turn_left(ANGLE)
wait_motors(motors)
motors.forward(DISTANCE)
wait_motors(motors)
# We can't turn more than 255 degrees because
# the data send to the Arduino is a 8bits value.
motors.turn_right(180)
wait_motors(motors)
motors.turn_right(90)
wait_motors(motors)
def dancer():
ZumoButton().wait_for_press()
m = Motors()
u = Ultrasonic()
print(u.getvalue())
m.forward(.2, 3)
m.backward(.2, 3)
m.right(.5, 3)
m.left(.5, 3)
m.backward(.3, 2.5)
m.set_value([.5, .1], 10)
m.set_value([-.5, -.1], 10)
def followTest():
ZumoButton().wait_for_press()
m = Motors()
follow = FollowLine()
time = 0
while (time < 100):
values = follow.isOnLine()
if values[0] == 0:
m.forward(0.2, 0.2)
elif values[1] == 0 or values[1] == 1:
m.left(0.2, 0.3)
else:
m.right(0.2, 0.3)
time += 1
def explorer(dist=10):
ZumoButton().wait_for_press()
m = Motors()
u = Ultrasonic()
print(u.sensor_get_value())
while u.sensor_get_value() > dist:
m.forward(.2, 0.2)
print(u.value)
m.backward(.1, .5)
m.left(.5,3)
m.right(.5, 3.5)
sleep(2)
while u.sensor_get_value() < dist*5:
m.backward(.2, 0.2)
m.left(.75, 5)
class Motob:
def __init__(self):
self.motors = Motors()
def update(self, motor_recommendation: MotorOperation):
print(motor_recommendation)
if motor_recommendation == MotorOperation.FORWARDS:
self.motors.forward()
elif motor_recommendation == MotorOperation.BACKWARDS:
self.motors.backward()
elif motor_recommendation == MotorOperation.TURN_LEFT:
self.motors.left(1, 2.5)
elif motor_recommendation == MotorOperation.TURN_RIGHT:
self.motors.right(1, 2.5)
else:
self.motors.forward(0)
class Server():
def __init__(self, host, port):
self.host = host
print(host)
self.port = port
self.motors = Motors()
def start(self):
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.sock.bind((self.host, self.port))
self.sock.listen(5)
return self.connection()
def connection(self):
conn, addr = self.sock.accept()
print(addr)
while True:
data = conn.recv(1024)
if data.decode("utf-8") == "w":
print('move forward')
self.motors.forward()
elif data.decode("utf-8") == "s":
print("move back")
self.motors.back()
elif data.decode("utf-8") == "a":
print("move left")
self.motors.left()
elif data.decode("utf-8") == "d":
print("move right")
self.motors.right()
elif data.decode("utf-8") == "space":
print("stopping")
self.motors.stop()
elif data.decode("utf-8") == "m":
print("server OFF")
break
elif data.decode("utf-8") == "q":
print("Program will close...")
self.sock.close()
return False
self.sock.close()
return True
def restart(self):
self.connection()
class Motob:
def __init__(self):
self.value = None
self.motors = Motors()
def update(self, v):
self.value = v
self.operate()
def operate(self):
if self.value[0] == 'S':
self.motors.stop()
elif self.value[0] == 'F':
self.motors.forward(0.5, 0.3)
elif self.value[0] == 'B':
self.motors.backward(0.3, 0.3)
elif self.value[0] == 'R':
self.motors.right(0.5, 0.3)
elif self.value[0] == 'L':
self.motors.left(0.5, 0.3)
class Motob:
def __init__(self):
self.motor = Motors()
def update(self, action, sleep_time = 0.33):
if action == "forward":
self.motor.forward()
sleep(sleep_time)
elif action == "backward":
self.motor.backward()
sleep(sleep_time * 6)
self.motor.left()
sleep(sleep_time / 10)
elif action == "left":
self.motor.left()
sleep(sleep_time / 10)
elif action == "right":
self.motor.right()
sleep(sleep_time / 10)
elif action == "charge":
self.motor.set_value((400, 400))
elif action == "brake":
self.motor.stop()
class Motob():
def __init__(self):
self.motor = Motors()
self.value = None # Most recent motor recommendation sent to the motob
def update(self, motor_recom):
self.value = motor_recom
self.operationalize()
def stop(self):
self.motor.stop()
def operationalize(self):
if self.value[0] == "left":
self.motor.stop()
self.motor.turn_left(self.value[1])
self.motor.forward(0.35)
elif self.value[0] == "right":
self.motor.stop()
self.motor.turn_right(self.value[1])
self.motor.forward(0.35)
elif self.value[0] == "drive":
self.motor.forward(self.value[1])
class Robot:
# US sensors constants.
US_SENSORS = [
{
'name': 'front_bottom',
'trigger_limit': 8,
'sensors': 0
},
{
'name': 'left',
'trigger_limit': 10,
'sensors': 2
},
{
'name': 'back',
'trigger_limit': 8,
'sensors': 4
},
{
'name': 'right',
'trigger_limit': 10,
'sensors': 3
},
{
'name': 'front_top',
'trigger_limit': 8,
'sensors': 1
},
]
# Take in count the obstacle dimension (assuming another robot)
# + the robot size.
OBSTACLES_DIMENSION = 50
def __init__(self):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
self._us = RangeSensor(4)
logging.info('Building the graph map.')
# self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
self._blocking_servo = -1
def reset_kinematics(self):
self._kinematic.up_clamp()
time.sleep(0.2)
self._kinematic.open_clamp()
time.sleep(0.2)
self._kinematic.reset_funny()
time.sleep(0.2)
self._kinematic.push_back()
time.sleep(1)
def wait_motors(self, enable=True, timeout=0):
first_time = time.time()
while not self._motors.is_done():
if enable:
if self._blocking_servo != -1:
i = self._blocking_servo
test = self._us.get_range(i)
print(test)
if test > self.US_SENSORS[i]['trigger_limit']:
self._motors.restart()
self._blocking_servo = -1
else:
continue
us_sensors = self._us.get_ranges()
print(us_sensors)
for i, us in enumerate(us_sensors):
if 'front' in self.US_SENSORS[i][
'name'] or 'back' in self.US_SENSORS[i]['name']:
if us < self.US_SENSORS[i]['trigger_limit']:
self._motors.stop()
self._blocking_servo = i
break
if (time.time() - first_time) > timeout and timeout != 0:
break
time.sleep(0.1)
def take_modules(self, number=1):
for i in range(number):
self._kinematic.open_clamp()
self._kinematic.down_clamp()
time.sleep(DELAY_UP_DOWN_CLAMP)
self._motors.forward(9)
self.wait_motors(enable=False, timeout=2)
self._kinematic.close_clamp()
time.sleep(1)
# self._motors.set_speed(40)
self._motors.backward(7)
if number == 1:
self._motors.forward(6)
self.wait_motors(enable=False, timeout=2)
self._motors.backward(6)
self.wait_motors(enable=False, timeout=2)
self.wait_motors(enable=False)
self._motors.backward(8)
self.wait_motors(enable=False)
#SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
self._kinematic.open_clamp()
time.sleep(0.5)
self._motors.forward(3)
self.wait_motors(enable=False, timeout=2)
self._kinematic.close_clamp()
time.sleep(1)
#---
self._motors.forward(3)
self._kinematic.up_clamp()
self.wait_motors(enable=False, timeout=3)
time.sleep(0.5)
self._kinematic.open_clamp()
time.sleep(1)
self._kinematic.down_clamp()
time.sleep(0.5)
self._kinematic.up_clamp()
def eject_modules(self, number=1):
for i in range(number):
self._kinematic.push_out()
time.sleep(1.5)
self._kinematic.push_back()
time.sleep(1.5)
self._motors.forward(2)
self.wait_motors(timeout=2)
self._motors.backward(8)
self.wait_motors(timeout=2)
def get_kin(self):
return self._kinematic
def get_motors(self):
return self._motors
def finalize(self):
self._motors.stop()
self._kinematic.launch_funny()
time.sleep(0.8)
self._kinematic.reset_funny()
class Robot:
_DELAY_OPEN_ClOSE_CLAMP = 0.7
_DELAY_UP_DOWN_CLAMP = 2.5
_DELAY_IN_OUT_BLOCK = 3
# US sensors constants.
US_SENSORS = [
{
'name': 'front_bottom',
'trigger_limit': 10,
'sensors': [0, 1]
},
{
'name': 'left',
'trigger_limit': 10,
'sensors': [2]
},
{
'name': 'back',
'trigger_limit': 5,
'sensors': [4]
},
{
'name': 'right',
'trigger_limit': 10,
'sensors': [3]
},
{
'name': 'front_top',
'trigger_limit': 10,
'sensors': [0, 1]
},
]
DIMENSION = {'length': 32.5, 'width': 19.2}
# Take in count the obstacle dimension (assuming another robot)
# + the robot size.
OBSTACLES_DIMENSION = 50
def __init__(self, position):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
self._position = position
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
logging.info('Building the graph map.')
robot_diagonal = math.sqrt(self.DIMENSION['length'] +
self.DIMENSION['width']**2)
self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
def take_modules(self, number=1, distance=0):
for i in range(number):
self._kinematic.down_clamp()
time.sleep(self._DELAY_UP_DOWN_CLAMP)
self._motors.forward(distance)
while not self._motors.is_done(self.__done_callback):
time.sleep(0.5)
self._kinematic.close_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
self._motors.backward(distance)
while not self._motors.is_done(self.__done_callback):
time.sleep(0.5)
#SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
self._kinematic.open_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
self._motors.forward(2.5)
while not self._motors.is_done(self.__done_callback):
time.sleep(0.5)
self._kinematic.close_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
#---
self._kinematic.up_clamp()
time.sleep(self._DELAY_UP_DOWN_CLAMP)
self._kinematic.open_clamp()
def eject_modules(self, number=1):
for i in range(number):
self._kinematic.push_out()
time.sleep(self._DELAY_IN_OUT_BLOCK)
if number == 4 and i == 2:
self._kinematic.open_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
self._kinematic.push_back()
time.sleep(self._DELAY_IN_OUT_BLOCK)
def move_to(self, target):
"""
target: a dict with keys:
- "angle": a X-axis relative constrain target angle.
- "point": position of the target.
"""
while True:
instructions = self._graph.get_path(self._position, target)
status = self._motors.move_with_instructions(
instructions, self.__move_callback, self.__done_callback)
if status == 'ok':
break
def finalize(self):
self._motors.stop()
self._kinematic.launch_funny()
time.sleep(1)
self._kinematic.reset_funny()
def __move_callback(self):
"""
Return a string giving the state if we need to stop or not the regulation because of
an obstacle.
"""
# ranges = self._us_sensors.get_ranges()
ranges = [20, 20, 20, 20, 20]
for us_data in self.US_SENSORS:
for i, sensor in enumerate(us_data['sensors']):
if ranges[sensor] == 0:
# Zero value means that we are too far from obstacles.
continue
if ranges[sensor] < us_data['trigger_limit']:
logging.warn(
'Motors stopped becauce of the %s%i US sensors at %i cm',
us_data['name'], i, ranges[sensor])
self._motors.stop()
# TODO: avoid recalculation if we have the near same position.
self._graph.reset_obstacles()
self._graph.add_obstacle(self._position, self.DIMENSION,
self.OBSTACLES_DIMENSION,
us_data['name'], ranges[sensor])
return 'obstacle'
return 'continue'
def __done_callback(self, distance_travelled, status='ok'):
"""
Update the position and angle informations of the robots.
"""
self._move_target = None
logging.info('Regulation is done!')
self.__update_robot_position(distance_travelled)
print(self._position)
return status
def __update_robot_position(self, distance_travelled):
left = distance_travelled['left']
right = distance_travelled['right']
if self.__is_opposite_sign(left, right):
# We finished a rotation regulation.
angle = ((right - left) / 2) / Motors.ANGLE_CORRECTION
self._position['angle'] += angle
logging.info('Robot turned with an angle of %i', angle)
else:
# We finished a lead regulation.
angle = self._position['angle']
distance = (left + right) / 2
self._position['point'][0] += distance * math.cos(
math.radians(angle))
self._position['point'][1] += distance * math.sin(
math.radians(angle))
def __is_opposite_sign(self, n, m):
if n > 0 and m > 0:
return False
elif n < 0 and m < 0:
return False
return True
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(.2, 3)
board.connect()
move = Motors()
mission = Mission()
mission.startMission()
board.connect()
print("Board connected")
sleep(0.5)
del1 = 5 # This is standard delay 1 in seconds
del2 = 3 # This is standard delay 2 in seconds
speed = 50 # This takes a value from 0 to 254
print("Will now go forward speed 50 for 1S")
sleep(0.8)
move.forward(50)
sleep(1)
move.stop()
print("Will now go backwards speed 50 for 1S")
sleep(1)
move.backward(50)
sleep(1)
move.stop()
print("robot will now move forward by 60 cm on button press")
sleep(1)
mission.startMission()
sleep(1)
move.position(60,100)
sleep(5)
move.stop()
from mission import Mission
board = Arduino()
board.connect()
move = Motors()
mission = Mission()
power_f = 100
power_t = 100
mission.startMission()
while True:
print("Forward")
move.forward(power_f)
sleep(4)
move.stop()
print("turn left")
move.leftMotor(power_f, 0)
move.rightMotor(power_f, 1)
sleep(1)
move.stop()
sleep(1)
print("turn right")
move.leftMotor(power_f, 1)
move.rightMotor(power_f, 0)
sleep(1)
move.stop()
from motors import Motors
connection = SerialManager(device='/dev/ttyUSB0')
try:
a = ArduinoApi(connection=connection)
except Exception as e:
print('ERROR CONNECTING TO ARDUINO')
exit(1)
motors = Motors()
motors.init_motors(a)
start = input('Any key to start')
print('Right wheel forward')
motors.forward(a, 0, 200)
sleep(1)
print('stopping...')
motors.stop(a)
sleep(.5)
print('Right wheel backward')
motors.reverse(a, 0, 200)
sleep(1)
print('stopping...')
motors.stop(a)
sleep(.5)
start = input('Any key to start')
print('Left wheel forward')
motors.forward(a, 200, 0)
sleep(1)
class Mission():
# tested
def __init__(self):
if pi is True:
self.board = Arduino()
self.board.connect()
self.move = Motors()
GPIO.setup(GREEN_LED_GPIO,GPIO.OUT)
GPIO.setup(RED_LED_GPIO,GPIO.OUT)
GPIO.setup(BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# load the sample hash
with open(rfid_hash) as fh:
self.rfid_hash = json.load(fh)
# won't test
def startMission(self):
if pi is True:
GPIO.output(GREEN_LED_GPIO,False)
GPIO.output(RED_LED_GPIO,True)
print("waiting for button...")
while GPIO.input(BUTTON) == False:
time.sleep(0.1)
GPIO.output(RED_LED_GPIO,False)
# won't test
def endMission(self):
if pi is True:
GPIO.output(GREEN_LED_GPIO,True)
# tested
def deleteData(self):
try:
os.unlink(data_file)
except OSError:
pass
# tested
# stores a dict as json on the disk
# appends to the data file
def saveData(self, sample):
with open(data_file,'a') as fh:
fh.write(json.dumps(sample) + "\n")
# tested
# returns a list of dicts
def loadData(self):
data = []
try:
with open(data_file,) as fh:
for sample in fh.readlines():
data.append(json.loads(sample))
except IOError:
pass
return data
# won't test
# fetches an RFID
def getLocation(self):
if pi is True:
rfid = ""
# keep moving until robot gets a location
while True:
rfid = self.board.sendCommand(Commands.READ_RFID,0,0)
# when I catalogued the RFIDs I missed the last char!
rfid = rfid[0:11]
if len(rfid) == 11:
break
self.move.forward(70)
time.sleep(0.1)
self.move.stop()
time.sleep(1)
return rfid
# tested
# indexes into the sample database with the RFID and returns sample as dict
def takeSample(self, location):
try:
return self.rfid_hash[location]
except KeyError:
raise Exception("unknown location [%s]" % location)
# tested
# uploads a sample dict with a team name (string)
def uploadSample(self, sample, team):
# fetch team ID from nane
r = requests.get(mc_url + '/api/team/' + team)
if r.status_code == 400:
raise Exception(r.text)
team_id = json.loads(r.text)['id']
sample['team'] = str(team_id)
# posts it
r = requests.post(mc_url + '/api/sample', json=sample)
if r.status_code == 400:
return r.text
#exeception stops the program
#raise Exception(r.text)
new_sample = json.loads(r.text)
print("uploaded sample %d" % new_sample['id'])
return new_sample
def getAllSamples(self):
r = requests.get(mc_url + '/api/samples')
samples = json.loads(r.text)['samples']
return samples
class Motob:
def __init__(self):
self.motor = Motors()
self.value = -1
self.speed = 0.35
self.funcs = {
"left": self.left,
"right": self.right,
"random": self.random,
"rewind": self.rewind
}
def update(self, recommendation):
print ('operating',recommendation,'in motobs')
self.operate(recommendation)
def operate(self, recommended):
if recommended == "None":
self.wander()
else:
func, self.value = recommended.split(" ")
self.value = int(self.value)
if func in self.funcs:
self.funcs[func]()
else:
self.wander()
def wander(self):
self.motor.forward(speed = self.speed, dur = 0.15)
def degree_to_duration(self, degrees):
# 0.75 per runde på full speed
# anta halv speed, så ca 1.5 sec per runde
time_per_round = 2.5
time_per_degree = time_per_round/360
return degrees*time_per_degree
def get_degrees(self):
return self.degree_to_duration(self.value)
def left(self):
self.motor.left(speed=self.speed*2, dur=self.get_degrees())
def right(self):
self.motor.right(speed=self.speed*2, dur=self.get_degrees())
def random(self):
# turn left or right by random
# turn 90 degrees regardless
self.value = 90
if randint(0,100)>50:
self.right()
else:
self.left()
def rewind(self):
# run back for a while
if self.value <= 0:
self.value = 1 # set a default value
self.motor.backward(speed=self.speed, dur = self.value)
