Skip to content
This repository has been archived by the owner on Feb 9, 2024. It is now read-only.

Architecture and camera updates #85

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Architecture and camera updates #85

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b2ff299
Architecture and calibration changes
polzounov Feb 26, 2022
df2c48a
Fixed problem with crosshairs
polzounov Feb 28, 2022
10bef63
Calibrate, pixel offsets all working
polzounov Mar 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
179 changes: 55 additions & 124 deletions sw/calibrate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
"""

import os
import cv2
import time
import json
import argparse
Expand All @@ -18,11 +17,11 @@

from env import MoabEnv
from typing import Tuple
from common import Vector2
from detector import hsv_detector
from controllers import pid_controller
from dataclasses import dataclass, astuple
from hardware import plate_angles_to_servo_positions
from detector import pixels_to_meters, meters_to_pixels
from common import Vector2, write_calibration, read_calibration


@dataclass
Expand All @@ -36,8 +35,8 @@ def __iter__(self):


@dataclass
class CalibPos:
position: Tuple[float, float] = (0.0, 0.0)
class CalibPixels:
offsets: Tuple[int, int] = (0, 0)
success: bool = False
early_quit: bool = False # If menu is pressed before the calibration is complete

Expand Down Expand Up @@ -106,24 +105,34 @@ def calibrate_hue(camera_fn, detector_fn, is_menu_down_fn):
return CalibHue()


def calibrate_pos(camera_fn, detector_fn, hue, is_menu_down_fn):
for i in range(10): # Try and detect for 10 frames before giving up
if is_menu_down_fn():
return CalibPos(early_quit=True)
def calibrate_pos(
camera_fn, detector_fn, get_buttons_fn, prev_pixel_offsets, sleep_time=1 / 30
):
def clip(x, low, high):
return max(0, min(high, x))

img_frame, elapsed_time = camera_fn()
ball_detected, ((x, y), radius) = detector_fn(img_frame, hue=hue)
img_frame, _ = camera_fn()
# Starting point for x and y pixels
x_pixels, y_pixels = prev_pixel_offsets
menu_button, joy_button, joy_x, joy_y = get_buttons_fn()

# If we found a ball roughly in the center that is large enough
if ball_detected and ball_close_enough(x, y, radius):
x_offset = round(x, 3)
y_offset = round(y, 3)
while True:
if menu_button:
return CalibPixels(early_quit=True)
if joy_button:
break

log.info(f"Offset calibrated: [{x_offset:.3f}, {y_offset:.3f}]")
return CalibPos(position=(x_offset, y_offset), success=True)
img_frame, _ = camera_fn()
lx, ly = img_frame.shape[:2]
x_pixels = int(x_pixels + round(joy_x * 2))
y_pixels = int(y_pixels + -round(joy_y * 2))
_ = detector_fn(img_frame, hue=0, debug=True, crosshairs=(x_pixels, y_pixels))

log.warning(f"Offset calibration failed.")
return CalibPos()
time.sleep(sleep_time)
menu_button, joy_button, joy_x, joy_y = get_buttons_fn()

log.info(f"Offset calibrated: ({x_pixels}, {y_pixels})")
return CalibPixels(offsets=(x_pixels, y_pixels), success=True)


def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20):
Expand All @@ -150,7 +159,7 @@ def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20):
vel_y_hist.append(vel_y)
prev_100_x = np.mean(np.abs(vel_x_hist[-100:]))
prev_100_y = np.mean(np.abs(vel_y_hist[-100:]))
print("Prev 100: ", (prev_100_x, prev_100_y))
# print("Prev 100: ", (prev_100_x, prev_100_y))

# If the average velocity for the last 100 timesteps is under the limit
if (prev_100_x < stationary_vel) and (prev_100_y < stationary_vel):
Expand All @@ -169,30 +178,6 @@ def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20):
return CalibServos()


def write_calibration(calibration_dict, calibration_file="bot.json"):
log.info("Writing calibration.")

# write out stuff
with open(calibration_file, "w+") as outfile:
log.info(f"Creating calibration file {calibration_file}")
json.dump(calibration_dict, outfile, indent=4, sort_keys=True)


def read_calibration(calibration_file="bot.json"):
log.info("Reading previous calibration.")

if os.path.isfile(calibration_file):
with open(calibration_file, "r") as f:
calibration_dict = json.load(f)
else: # Use defaults
calibration_dict = {
"ball_hue": 44,
"plate_offsets": (0.0, 0.0),
"servo_offsets": (0.0, 0.0, 0.0),
}
return calibration_dict


def wait_for_joystick_or_menu(hardware, sleep_time=1 / 30):
"""Waits for either the joystick or the menu. Returns the buttons"""
while True:
Expand All @@ -216,40 +201,38 @@ def run_calibration(env, pid_fn, calibration_file):
camera_fn = hardware.camera
detector_fn = hardware.detector

# Get old calibration (occasionally useful as a starting point)
calibration_dict = read_calibration(calibration_file)

def is_menu_down(hardware=hardware) -> bool:
return hardware.get_buttons().menu_button

# lift plate up first
hardware.set_angles(0, 0)

# Display message and wait for joystick
hardware.display(
"put ball on stand\nclick joystick",
# "Place ball in\ncenter using\nclear stand.\n\n" "Click joystick\nwhen ready."
scrolling=True,
)
buttons = wait_for_joystick_or_menu(hardware)
if buttons.menu_button: # Early quit
hardware.display("move crosshairs\nclick joystick", scrolling=True)

# Calibrate position
prev_pixel_offsets = calibration_dict["pixel_offsets"]
buttons = hardware.get_buttons
pos_calib = calibrate_pos(camera_fn, detector_fn, buttons, prev_pixel_offsets)
if pos_calib.early_quit:
hardware.go_up()
return

# Calibrate hue
hardware.display("put ball on stand\nclick joystick", scrolling=True)
hardware.display("Calibrating...")
hue_calib = calibrate_hue(camera_fn, detector_fn, is_menu_down)
if hue_calib.early_quit:
hardware.go_up()
return

# Calibrate position
pos_calib = calibrate_pos(camera_fn, detector_fn, hue_calib.hue, is_menu_down)
if pos_calib.early_quit:
hardware.go_up()
return

# Save calibration
calibration_dict = read_calibration(calibration_file)
calibration_dict["ball_hue"] = hue_calib.hue
calibration_dict["plate_offsets"] = pos_calib.position
x_offset, y_offset = pos_calib.position
calibration_dict["pixel_offsets"] = pos_calib.offsets
x_offset, y_offset = pos_calib.offsets
write_calibration(calibration_dict)

# Update the environment to use the new calibration
Expand All @@ -261,16 +244,18 @@ def is_menu_down(hardware=hardware) -> bool:
elif not (pos_calib.success or hue_calib.success): # or servo_calib.success):
hardware.display("Calibration failed\nClick menu...", scrolling=True)
else:
hue_str = (
f"Hue calib:\nsuccessful\nBall hue = {hue_calib.hue}\n\n"
if hue_calib.success
else "Hue calib:\nfailed\n\n"
)
pos_str = (
f"Position \ncalib:\nsuccessful\nPosition = \n({100*x_offset:.1f}, {100*y_offset:.1f})cm\n\n"
if hue_calib.success
else "(X, Y) calib:\nfailed\n\n"
)
# Calibration partially succeeded
if hue_calib.success:
hue_str = f"Ball hue = {hue_calib.hue}\n\n"
else:
hue_str = "Hue calib:\nfailed\n\n"

if pos_calib.success:
pos_str = f"Position = \n"
pos_str += f"({x_offset}, {y_offset})pix\n\n"
else:
pos_str = "(X, Y) calib:\nfailed\n\n"

hardware.display(
"Calibration\npartially succeeded\n\n"
+ hue_str
Expand Down Expand Up @@ -299,60 +284,6 @@ def is_menu_down(hardware=hardware) -> bool:
hardware.go_up()


def run_servo_calibration(env, pid_fn, calibration_file):
# Warning: servo calib works but doesn't currently give a good calibration
raise NotImplementedError

# Get some hidden things from env
hardware = env.hardware
camera_fn = hardware.camera
detector_fn = hardware.detector

# Start the calibration with uncalibrated servos
hardware.servo_offsets = (0, 0, 0)
# lift plate up fist
hardware.set_angles(0, 0)

# Calibrate servo offsets
hardware.display(
"Calibarating\nservos\n\n"
"Place ball in\ncenter without\n stand.\n\n"
"Click joystick\nto continue.",
scrolling=True,
)
buttons = wait_for_joystick_or_menu(hardware)
if buttons.menu_button: # Early quit
hardware.go_up()
return

hardware.display("Calibrating\nservos...", scrolling=True)
servo_calib = calibrate_servo_offsets(pid_fn, env)

# Save calibration
calibration_dict = read_calibration(calibration_file)
calibration_dict["servo_offsets"] = servo_calib.servos
s1, s2, s3 = servo_calib.servos
write_calibration(calibration_dict)

# Update the environment to use the new calibration
# Warning! This mutates the state!
env.reset_calibration(calibration_file=calibration_file)

if servo_calib.success:
hardware.display(
f"servo offsets =\n({s1:.2f}, {s2:.2f}, {s3:.2f})\n\n"
"Click menu\nto return...\n",
scrolling=True,
)
print(f"servo offsets =\n({s1:.2f}, {s2:.2f}, {s3:.2f})")
else:
hardware.display(
"Calibration\nfailed\n\nClick menu\nto return...", scrolling=True
)

hardware.go_up()


def calibrate_controller(**kwargs):
run_calibration(
kwargs["env"],
Expand Down
26 changes: 3 additions & 23 deletions sw/camera.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,11 @@
"""

import cv2
import time
import threading
import numpy as np

from time import time
from typing import Union, Tuple


# Link to raspicam_cv implementation for mapping values
Expand All @@ -22,8 +23,6 @@ def __init__(
brightness=60,
contrast=100,
frequency=30,
x_offset_pixels=0.0,
y_offset_pixels=0.0,
auto_exposure=True,
exposure=50, # int for manual (each 1 is 100µs of exposure)
):
Expand All @@ -38,9 +37,6 @@ def __init__(
self.exposure = exposure
self.prev_time = 0.0
self.source = None
self.last_frame = None
self.x_offset_pixels = x_offset_pixels
self.y_offset_pixels = y_offset_pixels

def start(self):
self.source = cv2.VideoCapture(self.device_id)
Expand Down Expand Up @@ -69,28 +65,12 @@ def __call__(self):
raise Exception("Using camera before it has been started.")

# Calculate the time elapsed since the last sensor snapshot
curr_time = time()
curr_time = time.time()
elapsed_time = curr_time - self.prev_time
self.prev_time = curr_time

ret, frame = self.source.read()
if ret:
w, h = 384, 288
d = 256 # Our final "destination" rectangle is 256x256

# Ensure the offset crops are possible
x_offset_pixels = min(self.x_offset_pixels, (w / 2 - d / 2))
x_offset_pixels = max(self.x_offset_pixels, -(w / 2 - d / 2))
y_offset_pixels = min(self.y_offset_pixels, (h / 2 - d / 2))
y_offset_pixels = max(self.y_offset_pixels, -(h / 2 - d / 2))

# Calculate the starting point in x & y
x = int((w / 2 - d / 2) + x_offset_pixels)
y = int((h / 2 - d / 2) + y_offset_pixels)

frame = frame[y : y + d, x : x + d]
# frame = frame[:-24, 40:-80] # Crop so middle of plate is middle of image
# cv2.resize(frame, (256, 256)) # Crop off edges to make image (256, 256)
return frame, elapsed_time
else:
raise ValueError("Could not get the next frame")
Loading