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:
"""
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])
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.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 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 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 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")
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 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)
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
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:
"""Interface between a behavior and one or more motors."""
def __init__(self):
"""Initialize Motob object.
Parameters
----------
action : Holder of the action whose settings will be determined by the motob.
duration : Holder of the most recent duration the of the action.
"""
self.m = Motors()
self.action = None
self.duration = None
def update(self, recommendation):
# print('Motor recommendation:', recommendation)
"""Update object.
Receive a new motor recommendation.
Load it into the instance variables.
Operationalize it.
Example
-------
recommendation = ('L', 1)
"""
self.m.stop()
self.action = recommendation[0]
self.duration = recommendation[1]
self.operationalize(self.action, self.duration)
def operationalize(self, action, duration):
"""Convert motor recommendation into motor setting and send to corresponding motor(s).
Parameters
----------
action : Action the motors will perform.
dur : Duration of the operation.
Example of actions
------------------
F = Drive forward
B = Drive backward
Z = Boost forward
X = Flee
T = Turn around 180 degrees
L = Turn left while driving forward
R = Turn right while driving forward
S = Stop
"""
if action == 'F':
self.m.set_value((0.5, 0.5), duration)
elif action == 'B':
self.m.backward(0.25, duration)
elif action == 'Z':
self.m.set_value((1, 1), duration)
elif action == 'X':
self.m.flee() # Turn around, must be tuned for 180 degree turn.
elif action == 'T':
self.m.turn()
elif action == 'L':
self.m.left((0.1, 0.25), duration)
elif action == 'R':
self.m.right((0.25, 0.1), duration)
elif action == 'S':
self.m.stop()
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)
