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 __init__(self, verbose=False):
self.verbose = verbose
self.status_counter = 0
self.state = self.STATE_WAITING
self.currenttime = 0
self.flex_fish_limit = self.FLEX_FISH_LIMIT
self.player = Player(self, verbose)
self.adc_sensors = AdcSensors(self, verbose)
self.motors = Motors(self, verbose)
self.web_connection = WebConnection(self, verbose)
self.gps_tracker = GpsTracker(self, verbose)
self.video = Video(self, verbose)
# Initial valus for settings
# Speed: (0-1). 1 = Full speed ahead
# Turn = -1 - +1 = +1 Only left motor on (turn to right)
# 0 Both motors same speed
# -1 Only right motor on (turn to left)
self.speed = 0.0
self.turn = 0.0
# speed style examples:
# - Constant speed = (low_speed_percent = 100)
# - Stop and go jigging with 6 sec motor on and 4 sec stop. low_speed_percent = 0,speed_change_cycle = 10, speed_motors_full_percent = 60
# - Trolling with 10 sec half speed and 5 sec full speed. low_speed_percent = 50, speed_change_cycle = 15, speed_motors_full_percent = 66.66
self.speed_change_cycle = 0
self.speed_motors_full_percent = 100
self.low_speed_percent = 0
# Play music or not
self.play_music = False
def lineTest():
reflect = ReflectanceSensors()
ZumoButton().wait_for_press()
motor = Motors()
stuck = CheckStuck()
camera = Camera()
motob = Motob(motor)
arbitrator = Arbitrator(motob=motob)
sensob = Sensob()
sensob.set_sensors([reflect])
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
bbcon.add_sensob(sensob)
bbcon.set_checkStucker(stuck)
b = LineFollow(1, [sensob])
bbcon.add_behavior(b)
timesteps = 0
while timesteps < 30:
bbcon.run_one_timestep()
timesteps += 1
def lineCollision():
reflect = ReflectanceSensors()
ZumoButton().wait_for_press()
motor = Motors()
stuck = CheckStuck()
ultra = Ultrasonic()
proxim = IRProximitySensor()
sensobCol = Sensob()
sensobCol.set_sensors([ultra, proxim])
motob = Motob(motor=motor)
sensobLine = Sensob()
sensobLine.set_sensors([reflect])
arb = Arbitrator(motob=motob)
bbcon = BBCON(arbitrator=arb, motob=motob)
bbcon.set_checkStucker(stuck)
line = LineFollow(1, [sensobLine])
col = CollisionAvoidance(1, [sensobCol])
bbcon.add_behavior(line)
bbcon.add_behavior(col)
bbcon.add_sensob(sensobCol)
bbcon.add_sensob(sensobLine)
count = 0
while count < 20:
bbcon.run_one_timestep()
count += 1
def start():
bbcon = BBCON()
arb = Arbitrator(bbcon)
motor = Motors()
reflect_sens = ReflectanceSensors(False)
cam = Camera()
ir = IR()
ultra = Ultrasonic()
bbcon.add_motobs(motor)
bbcon.set_arb(arb)
bbcon.add_behaviour(AvoidObj(bbcon, ultra, ir))
bbcon.add_behaviour(Behaviour_line_follower(bbcon, reflect_sens))
bbcon.add_behaviour(fub(bb=bbcon))
#behaviour avoid blue, has to be added last, do not change this, will screw up bbcon code
bbcon.add_behaviour(Behaviour_avoid_blue(bb=bbcon, cam=cam, ultra=ultra))
bbcon.add_sensob(reflect_sens)
bbcon.add_sensob(ir)
bbcon.add_sensob(ultra)
#cam has to be added last, will screw up bbcon code if not
bbcon.add_sensob(cam)
butt = ZumoButton()
butt.wait_for_press()
print("Start zumo")
while True:
bbcon.run_one_timestep()
def test1():
ZumoButton().wait_for_press()
m = Motors()
m.turn_right(90)
m.turn_left(180)
m.turn_right(360)
m.turn_left(360)
def __init__(self):
self.motors = Motors()
#self.marg = MARG()
# not sure if I want this here, but will add for now
self.server = Server()
self.prev_msg = b''
def __init__(self):
self.ub.Init()
self.tb.Init()
self.t1 = datetime.now()
self.tickSpeed = 0.05
self.us = Ultrasonics(self.ub)
self.motors = Motors(self.tb, self.ub, self.tickSpeed)
self.steering = Steering(self.tb, self.ub, self.tickSpeed)
self.teleLogger = Telemetry("telemetry", "csv").get()
self.ptc = PanTiltController(self.ub, 270, 135)
self.joystick_controller = JoystickController(self.tb, self.ub)
battMin, battMax = self.tb.GetBatteryMonitoringLimits()
battCurrent = self.tb.GetBatteryReading()
print('Battery monitoring settings:')
print(' Minimum (red) %02.2f V' % (battMin))
print(' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2))
print(' Maximum (green) %02.2f V' % (battMax))
print()
print(' Current voltage %02.2f V' % (battCurrent))
print(' Mode %s' % (self.mode))
print()
def __init__(self):
# Initialize arbitrator
self.arbitrator = Arbitrator()
# Initialize motobs (single object for both motors on the Zumo)
self.motobs.append(Motob(Motors()))
# Initialize sensors
self.sensors = {
'ultrasonic': Ultrasonic(0.05),
'IR': IRProximitySensor(),
'reflectance': ReflectanceSensors(False, 0, 900),
'camera': Camera(),
}
self.active_sensors = [self.sensors['ultrasonic'], self.sensors['IR'], self.sensors['reflectance']]
# Initialize sensobs
self.sensobs = {
'distance': DistanceSensob([self.sensors['ultrasonic']]),
'line_pos': LinePosSensob([self.sensors['reflectance']]),
'proximity': ProximitySensob([self.sensors['IR']]),
'red_search': RedSearchSensob([self.sensors['camera']]),
}
self.active_sensobs = [self.sensobs['distance'], self.sensobs['line_pos'], self.sensobs['proximity']]
time.sleep(1)
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
def __init__(self):
self.arbit = Arbitrary()
self.value = None
self.motorList = []
self.motorList.append(Motors())
def vsvingsakte():
speed = 0.5
ZumoButton().wait_for_press()
motors = Motors()
motors.set_value((speed / 2, speed / 2), 1)
motors.set_value((speed, -speed), 0.1)
motors.set_value((speed / 2, speed / 2), 1)
def vsvingrask():
speed = 0.5
ZumoButton().wait_for_press()
motors = Motors()
motors.set_value((speed / 2, speed / 2), 1)
motors.set_value((0.3, -0.3), 1)
motors.set_value((speed / 2, speed / 2), 1)
def trackTest():
ZumoButton().wait_for_press()
motor = Motors()
ultra = Ultrasonic()
camera = Camera()
stuck = CheckStuck()
motob = Motob(motor)
arbitrator = Arbitrator(motob=motob)
sensob = Sensob()
sensob.set_sensors([ultra, camera])
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
b = TrackObject(priority=1, sensobs=[sensob])
bbcon.set_checkStucker(stuck)
bbcon.add_behavior(b)
bbcon.activate_behavior(0)
bbcon.add_sensob(sensob)
timesteps = 0
while timesteps < 25:
bbcon.run_one_timestep()
timesteps += 1
def shoot_panorama(camera=Camera(),motors=Motors(),shots=5):
s = 1
im = IMR.Imager(image=camera.update()).scale(s,s)
rotation_time = 3/shots # At a speed of 0.5(of max), it takes about 3 seconds to rotate 360 degrees
for i in range(shots-1):
motors.right(0.5,rotation_time)
im = im.concat_horiz(IMR.Imager(image=camera.update()))
return im
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 tourist(steps=25,shots=5,speed=.25):
ZumoButton().wait_for_press()
rs = ReflectanceSensors(); m = Motors(); c = Camera()
for i in range(steps):
random_step(m,speed=speed,duration=0.5)
vals = rs.update()
if sum(vals) < 1: # very dark area
im = shoot_panorama(c,m,shots)
im.dump_image('vacation_pic'+str(i)+'.jpeg')
def operationalize(self):
#Oppretter motors-objekt
m = Motors()
#henter inn recommendation fra arbitrator og plugger inn i motors
m.set_value(self.values, 0.5)
#Går gjennom listen og bruker den til å utføre de forskjellige handlingene fra motors
for i in range(len(self.motors)):
self.motors[i].set_value(self.values, 0.5)
def bbrun():
# Main function sets up motobs/sensobs and behavors
sensobs = []
# Calibration begins here. Students should slowly spin the robot around the
# line in a circle trying hard not to lift it up off the ground. As of now,
# calibration lasts 15 seconds which is plenty of time. Without proper
# calibration, the line following behavior may not work properly.
# Calibration determines the maximum and minimum values found by each sensor,
# which is necessary for returning the normalized values (reals from 0 to 1)
# of each sensor.
# Edit: I have turned off calibration for now, because hard-coding the configuration
# is working good enough and I don't have to use time on calibrating under testing
motors = Motors()
motors.stop()
sleep(2)
# To exit the program properly on keyboardinterrupt
def signal_handler(signal, frame):
motors.stop()
GPIO.cleanup()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
reflectanceSensob = bbc.Sensob(ReflectanceSensors())
cameraSensob = bbc.Sensob(Camera())
sensobs.append(reflectanceSensob)
sensobs.append(cameraSensob)
motob = bbc.Motob(motors)
bbcon = RobotController(None) # None agent
# Create the behaviors
# The behaviors has priorities of 3,3,5,and 1 respectively
followLineBehavior = FollowLine(bbcon, sensobs[0:1])
camera_search_behavior = Camera_search(bbcon, sensobs[1:2])
stopFollowingLineBehavior = StopFollowLine(bbcon, sensobs[0:1])
wanderBehavior = Wander(bbcon)
bbcon.add_behavior(followLineBehavior)
bbcon.add_behavior(stopFollowingLineBehavior)
bbcon.add_behavior(wanderBehavior)
bbcon.add_behavior(camera_search_behavior)
bbcon.add_motob(motob)
bbcon.add_sensob(reflectanceSensob)
bbcon.add_sensob(cameraSensob)
i = 0
while not bbcon.is_blob_found():
print("Iteration " + str(i))
i += 1
print("Found gate: " + str(bbcon.found_gate))
bbcon.run_one_timestep()
# Called at the end
motors.stop()
def __init__(self):
self.motor = Motors() #list of all motors whose values it sets
self.value = [] #most recent motor recommendation sent to the motob.
self.motorsettings = {
'f': self.motor.forward,
'b': self.motor.backward,
'l': self.motor.left,
'r': self.motor.right,
's': self.motor.stop
}
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 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 __init__(self, host, port):
self.host = host
self.port = port
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.bind((host, port))
self.socket.listen(1)
self.client_socket = self.socket.accept()[0]
Motors()
self.listen()
def __init__(self, a_star=None):
self.a_star = a_star or AStar()
self.encoders = Encoders(self.a_star)
self.odometer = Odometer(self.encoders)
self.motors = Motors(self.a_star, self.odometer)
self.camera = Camera()
self.log = logging.getLogger('romi')
self.motors.stop()
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 main():
gamepad = Gamepad()
motors = Motors()
iks = []
for i in range(4):
iks.append(DeltaIK())
controller = ManualController(iks)
angles = 8 * [0.0]
kPs = 8 * [0.1]
frametime = 1.0 / 30.0
print("Ready.")
while True:
try:
s = time.time()
if not gamepad.process_events():
print("Controller disconnected, waiting for reconnect...")
while not gamepad.reconnect():
time.sleep(0.5)
print("Controller reconnected.")
if gamepad.is_a_down():
print("Shutdown button (A) has been pressed, shutting down...")
break
pad_y = gamepad.get_left_thumbstick_y()
controller.step(pad_y, frametime)
# Map IK leg results to motors
for i in range(4):
angles[legMap[i]
[0]] = legDirs[i][0] * (iks[i].angle -
(np.pi / 2.0 - iks[i].A))
angles[legMap[i]
[1]] = legDirs[i][1] * (iks[i].angle +
(np.pi / 2.0 - iks[i].A))
motors.sendCommands(angles, kPs)
time.sleep(max(0, frametime - (time.time() - s)))
except Exception as e:
print(e)
break
print("-- Program at End --")
async def update_motors():
global cmd
m = Motors()
m.set(0, 0)
while True:
if time.time() - cmd['time'] > p.motor_command_timeout:
m.set(0, 0)
else:
m.set(cmd['forward'], cmd['turn'])
await asyncio.sleep(0.05)
def shoot_panorama(shots=5):
camera = Camera()
motors = Motors()
s = 1
im = IMR.Imager(image=camera.update()).scale(s, s)
rotation_time = 3 / shots # At a speed of 0.5(of max), it takes about 3 seconds to rotate 360 degrees
for i in range(shots - 1):
motors.right(0.5, rotation_time)
im = im.concat_horiz(IMR.Imager(image=camera.update()))
im.dump_image("/root/Oving6_Plab/basic_robot/bilder/test.png")
return im
def __init__(self, debug=False):
self.motor = Motors()
self.behaviors = []
self.debug = debug
self.sensobs = {
Camera(img_width=IMG_WIDTH, img_height=IMG_HEIGHT): None,
Ultrasonic(): None,
ReflectanceSensors(): None
}
self.arbitrator = None
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)
