def infrare_handler(tup):
global time_eclipse, rush
left, middle, right = tup
if middle == 0:
time_eclipse = 0
rush = False
car.brake()
car.remove_infrared_sensor_change(infrare_handler)
def rush_ball():
global time_eclipse, rush
car.go()
time.sleep(0.8)
time_eclipse = 0
rush = False
car.brake()
car.remove_infrared_sensor_change(infrare_handler)
def move_around_ball_counterclockwise():
global direction
direction = False
car.rotate_right_90()
car.on_infrared_sensor_change(brake_if_touch_something)
car.go()
time.sleep(0.2)
car.brake()
def center_ball(bound, resolution, go=False):
global direction, time_eclipse
center_x = bound[0] + bound[2] / 2
if resolution[0] / 2 - threshold <= center_x <= resolution[
0] / 2 + threshold:
car.brake()
if go:
go_ball(bound)
else:
time_eclipse = 0
return
elif 0 < center_x < resolution[0] / 2 - threshold:
if not direction:
time_eclipse = max(time_eclipse / 2, 0.005)
direction = True
elif center_x > resolution[0] / 2 + threshold:
if direction:
time_eclipse = max(time_eclipse / 2, 0.005)
direction = False
if not rush:
if time_eclipse == 0:
time_eclipse = 0.05
if direction:
car.rotate_right_in_place()
time.sleep(time_eclipse)
car.brake()
else:
car.rotate_left_in_place()
time.sleep(time_eclipse)
car.brake()
# wait for camera stable
time.sleep(0.005)
def center_ball(camera, threshold=10, max_rotating_time=3.0):
"""
Adjust the position of car.
Make the ball is at the center of image captured by camera.
"""
global direction # True means right
rotate_interval = 0.05
total_rotating_time = 0.0
result = None
resolution = camera.resolution
rawCapture = picamera.array.PiRGBArray(camera, size=camera.resolution)
for frame in camera.capture_continuous(rawCapture,
format='bgr',
use_video_port=True):
# fetch an image from camera, then find the circle in it
_, bound = find_circle(frame.array, required=False)
# decides what to do
center_x = bound[0] + bound[2] / 2
if resolution[0] / 2 - threshold <= center_x <= resolution[
0] / 2 + threshold:
result = center_x
car.brake()
break
elif 0 < center_x < resolution[0] / 2 - threshold:
if not direction:
rotate_interval = max(rotate_interval / 2, 0.005)
direction = True
elif center_x > resolution[0] / 2 + threshold:
if direction:
rotate_interval = max(rotate_interval / 2, 0.005)
direction = False
# do rotate car in place
if direction:
car.rotate_right_in_place()
time.sleep(rotate_interval)
total_rotating_time += rotate_interval
car.brake()
else:
car.rotate_left_in_place()
time.sleep(rotate_interval)
total_rotating_time += rotate_interval
car.brake()
# wait for camera stable
time.sleep(0.005)
rawCapture.truncate()
rawCapture.seek(0)
if total_rotating_time >= max_rotating_time:
break
# returns the final x center of the ball
return result
def step(actions):
""" Execute one simulation step. The ``actions`` parameter is
a list of acceleration (>=0) or braking (<0) coefficients
to apply to each car.
"""
global positions, speeds # necessary in order to change these variables
newPositions = []
newSpeeds = []
for position, speed, action in zip(positions, speeds, actions):
newPositions.append(car.drive(position, speed))
if action >= 0:
newSpeed = car.accelerate(speed, action)
else:
newSpeed = car.brake(speed, -action)
newSpeeds.append(newSpeed)
positions = newPositions
speeds = newSpeeds
import car
car = car.Car(2005, "Audi", "A4")
print("car accelerates")
car.accelerate()
print(car.get_speed())
car.accelerate()
print(car.get_speed())
car.accelerate()
print(car.get_speed())
car.accelerate()
print(car.get_speed())
car.accelerate()
print(car.get_speed())
print("car decelerates")
car.brake()
print(car.get_speed())
car.brake()
print(car.get_speed())
car.brake()
print(car.get_speed())
car.brake()
print(car.get_speed())
car.brake()
print(car.get_speed())
def unload():
car.brake()
def brake_if_touch_something(status):
if status[0] == 0 or status[1] == 0 or status[2] == 0:
car.brake()
car.remove_infrared_sensor_change(brake_if_touch_something)
def on_message(self, message):
global car_mode
if message == 'ping':
# Reply with status of sensors
infrared = car._last_infrared_sensor_status
tracks = car._last_track_detector_status
reply = []
reply.append({
'type': 'sensor',
'data': {
'Left Infrared Sensor': infrared[0],
'Middle Infrared Sensor': infrared[1],
'Right Infrared Sensor': infrared[2],
'Left Track Detector': tracks[0],
'Middle Track Detector': tracks[1],
'Right Track Detector': tracks[2]
}
})
while not ws_tasks.empty():
reply.append(ws_tasks.get())
self.write_message(json.dumps(reply))
else:
print("WebSocket on_message(not ping)")
print(message)
message = json.loads(message)
if message['mode'] == 'track':
if car_mode == 'ball':
print('Stop find ball')
if camera is not None:
camera.origin()
print('Start find track')
car_mode = 'track'
car.set_global_speed(10)
find_track_and_avoid.load()
car.go()
elif message['mode'] == 'ball':
if car_mode == 'track':
print('Stop find track')
find_track_and_avoid.unload()
print('Start find ball')
car_mode = 'ball'
car.set_global_speed(10)
if camera is not None:
camera.mark()
ball_utils.load()
elif message['mode'] == 'user':
if car_mode == 'track':
print('Stop find track')
find_track_and_avoid.unload()
elif car_mode == 'ball':
print('Stop find ball')
if camera is not None:
camera.origin()
ball_utils.unload()
car_mode = 'user'
if message['speed'] == 0:
car.brake()
else:
car.set_global_speed(message['speed'])
if message['direction'] == 0:
car.go()
elif message['direction'] == 180:
car.back()
elif message['direction'] == 270:
car.rotate_left()
elif message['direction'] == 90:
car.rotate_right()
else:
print('Invalid mode "{}", ignored.'.format(message['mode']))
def on_close(self):
car.brake()
print("WebSocket on_closed")
