class MoabEnv:
def __init__(
self,
frequency=30,
debug=False,
verbose=0,
derivative_fn=derivative,
calibration_file="bot.json",
):
self.debug = debug
self.verbose = verbose
self.frequency = frequency
self.derivative_fn = derivative
self.vel_x = self.derivative_fn(frequency)
self.vel_y = self.derivative_fn(frequency)
self.sum_x, self.sum_y = 0, 0
self.hat = Hat(debug=debug, verbose=verbose)
self.hat.open()
self.camera = OpenCVCameraSensor(frequency=frequency)
self.detector = hsv_detector(debug=debug)
self.calibration_file = calibration_file
self.reset_calibration()
def __enter__(self):
self.camera.start()
return self
def __exit__(self, type, value, traceback):
self.hat.go_down()
self.hat.disable_servos()
self.hat.display_power_symbol("TO WAKE", PowerIcon.POWER)
self.hat.close()
self.camera.stop()
def __repr__(self):
return self.__str__()
def __str__(self):
return f"hue: {self.hue}, offsets: {self.servo_offsets}"
def reset_calibration(self, calibration_file=None):
# Use default if not defined
calibration_file = calibration_file or self.calibration_file
# Get calibration settings
if os.path.isfile(calibration_file):
with open(calibration_file, "r") as f:
calib = json.load(f)
else: # Use defaults
calib = {
"ball_hue": 44,
"plate_offsets": (0.0, 0.0),
"servo_offsets": (0.0, 0.0, 0.0),
}
plate_offsets = calib["plate_offsets"]
self.plate_offsets_pixels = meters_to_pixels(plate_offsets)
self.servo_offsets = calib["servo_offsets"]
self.hue = calib["ball_hue"]
# Set the servo offsets (self.hue & self.plate_offsets_pixels are used in step)
self.hat.set_servo_offsets(*self.servo_offsets)
self.camera.x_offset_pixels = self.plate_offsets_pixels[0]
self.camera.y_offset_pixels = self.plate_offsets_pixels[1]
def reset(self, text=None, icon=None):
# Optionally display the controller active text
if icon and text:
self.hat.display_string_icon(text, icon)
elif text:
self.hat.display_string(text)
# Reset the derivative of the position
# Use a high pass filter instead of a numerical derivative for stability.
# A high pass filtered signal can be thought of as a derivative of a low
# pass filtered signal: fc*s / (s + fc) = fc*s * 1 / (s + fc)
# For more info: https://en.wikipedia.org/wiki/Differentiator
# Or: https://www.youtube.com/user/ControlLectures/
self.vel_x = self.derivative_fn(self.frequency)
self.vel_y = self.derivative_fn(self.frequency)
# Reset the integral of the position
self.sum_x, self.sum_y = 0, 0
# Return the state after a step with no motor actions
return self.step((0, 0))
def step(self, action) -> Tuple[EnvState, bool, Buttons]:
plate_x, plate_y = action
self.hat.set_angles(plate_x, plate_y)
frame, elapsed_time = self.camera()
ball_detected, cicle_feature = self.detector(frame, hue=self.hue)
ball_center, ball_radius = cicle_feature
x, y = ball_center
# Update derivate calulation
vel_x, vel_y = self.vel_x(x), self.vel_y(y)
# Update the summation (integral calculation)
self.sum_x += x
self.sum_y += y
buttons = self.hat.get_buttons()
state = EnvState(x, y, vel_x, vel_y, self.sum_x, self.sum_y)
return state, ball_detected, buttons
class MoabHardware:
def __init__(
self,
frequency=30,
debug=False,
verbose=0,
calibration_file="bot.json",
):
self.debug = debug
self.verbose = verbose
self.frequency = frequency
self.hat = Hat(debug=debug, verbose=verbose)
self.hat.open()
self.camera = OpenCVCameraSensor(frequency=frequency)
self.detector = hsv_detector(debug=debug)
# Set the calibration
self.calibration_file = calibration_file
self.reset_calibration()
def __enter__(self):
self.camera.start()
return self
def __exit__(self, type, value, traceback):
self.go_down()
self.hat.disable_servos()
self.hat.display_power_symbol("TO WAKE", PowerIcon.POWER)
self.hat.close()
self.camera.stop()
def __repr__(self):
return self.__str__()
def __str__(self):
return f"hue: {self.hue}, servo offsets: {self.servo_offsets}, plate offsets: {self.plate_offsets}"
def reset_calibration(self, calibration_file=None):
# Use default if not defined
calibration_file = calibration_file or self.calibration_file
# Get calibration settings
if os.path.isfile(calibration_file):
with open(calibration_file, "r") as f:
calib = json.load(f)
else: # Use defaults
calib = {
"ball_hue": 44,
"plate_offsets": (0.0, 0.0),
"servo_offsets": (0.0, 0.0, 0.0),
}
self.plate_offsets = calib["plate_offsets"]
self.servo_offsets = calib["servo_offsets"]
self.hue = calib["ball_hue"]
def go_up(self):
# Set the plate to its hover position, experimentally found to be 150
self.set_servos(150, 150, 150)
time.sleep(
0.200) # Give enough time for the action before turning off servos
def go_down(self):
# Set the plate to its lowest position, experimentally found to be 155
self.set_servos(155, 155, 155)
time.sleep(
0.200) # Give enough time for the action before turning off servos
def get_buttons(self):
# Get the buttons after doing a noop (ensure the buttons are up to date)
self.hat.noop()
return self.hat.get_buttons()
def enable_servos(self):
self.hat.enable_servos()
def disable_servos(self):
self.hat.disable_servos()
def set_servos(self, s1, s2, s3):
# Note the indexing offset
s1 += self.servo_offsets[0]
s2 += self.servo_offsets[1]
s3 += self.servo_offsets[2]
self.hat.set_servos((s1, s2, s3))
def display(
self,
text: Optional[str] = None,
icon: Optional[str] = None,
scrolling: bool = False,
):
# Optionally display the controller active text
if icon and text:
if scrolling:
raise ValueError(
"Cannot display scrolling screen with an icon")
self.hat.display_string_icon(text, icon)
elif text:
if scrolling:
self.hat.display_long_string(text)
else:
self.hat.display_string(text)
def set_angles(self, pitch, roll):
s1, s2, s3 = plate_angles_to_servo_positions(pitch, roll)
self.set_servos(s1, s2, s3)
def step(self, pitch, roll) -> Buttons:
self.set_angles(pitch, roll)
frame, elapsed_time = self.camera()
buttons = self.hat.get_buttons()
ball_detected, (ball_center, ball_radius) = self.detector(frame,
hue=self.hue)
return ball_center, ball_detected, buttons