81 lines
2.5 KiB
Python
81 lines
2.5 KiB
Python
import cv2
|
|
import time
|
|
import board
|
|
import adafruit_bno055
|
|
import csv
|
|
import numpy as np
|
|
from math import atan2, pi, cos, sin, pi
|
|
|
|
|
|
i2c = board.I2C()
|
|
sensor = adafruit_bno055.BNO055_I2C(i2c)
|
|
|
|
points = []
|
|
|
|
def remap(value, maxInput, minInput, maxOutput, minOutput):
|
|
value = maxInput if value > maxInput else value
|
|
value = minInput if value < minInput else value
|
|
inputSpan = maxInput - minInput
|
|
outputSpan = maxOutput - minOutput
|
|
scaledThrust = float(value - minInput) / float(inputSpan)
|
|
return minOutput + (scaledThrust * outputSpan)
|
|
|
|
def draw_frame(heading, mag_data):
|
|
mag_x, mag_y = mag_data
|
|
global points
|
|
frame = np.full(shape=[400, 400, 3], fill_value=255, dtype=np.uint8)
|
|
length = 150
|
|
P1 = (200, 200)
|
|
P2_0 = int(P1[0] + length * cos(heading * pi / 180.0))
|
|
P2_1 = int(P1[1] + length * sin(heading * pi / 180.0))
|
|
P2 = (P2_0, P2_1)
|
|
|
|
frame = cv2.line(frame,P1,P2,(255,0,0),2)
|
|
frame = cv2.circle(frame, (mag_x, mag_y), radius=1, color=(0, 255, 0), thickness=3)
|
|
|
|
for p in points:
|
|
frame = cv2.circle(frame, (p[0], p[1]), radius=1, color=(0, 0, 255), thickness=3)
|
|
points.append((mag_x, mag_y))
|
|
frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
|
|
|
|
font = cv2.FONT_HERSHEY_SIMPLEX
|
|
org_n = (200, 30)
|
|
org_s = (200, 370)
|
|
org_w = (30, 200)
|
|
org_e = (370, 200)
|
|
fontScale = 1
|
|
color = (255, 0, 0)
|
|
thickness = 2
|
|
frame = cv2.putText(frame, 'N', org_n, font, fontScale, color, thickness, cv2.LINE_AA)
|
|
frame = cv2.putText(frame, 'S', org_s, font, fontScale, color, thickness, cv2.LINE_AA)
|
|
frame = cv2.putText(frame, 'W', org_w, font, fontScale, color, thickness, cv2.LINE_AA)
|
|
frame = cv2.putText(frame, 'E', org_e, font, fontScale, color, thickness, cv2.LINE_AA)
|
|
|
|
cv2.imshow('direction', frame)
|
|
|
|
def get_heading():
|
|
try:
|
|
mag_data = sensor.magnetic
|
|
for i in [0, 1]:
|
|
if abs(mag_data[i]) > 20:
|
|
print(f"Got {mag_data[i]} on {'X' if i == 0 else 'Y' if i==1 else 'Z'}, skipping")
|
|
return False
|
|
heading = 180 * atan2(mag_data[1],mag_data[0])/pi
|
|
if heading < 0:
|
|
heading += 360
|
|
mag_x = int(remap(mag_data[0], 20, -20, 0, 400))
|
|
mag_y = int(remap(mag_data[1], 20, -20, 0, 400))
|
|
print(f'Heading: {heading}')
|
|
return (heading+180, (mag_x, mag_y))
|
|
except Exception as e:
|
|
print(f"Encountered exception: {e}")
|
|
return False
|
|
|
|
while True:
|
|
heading = get_heading()
|
|
if heading:
|
|
draw_frame(heading[0], heading[1])
|
|
cv2.waitKey(1)
|
|
|
|
time.sleep(0.2)
|