def checkSet():
image_data = IT.takeImage()
if image_data == []:
print("return")
return
theta = IT.getAngle(image_data)
print("theta1 = ", theta)
Motors.turnDegrees(theta)
time.sleep(4)
def angleAdjust(dist_target):
QR_FOUND = True
image_data = IT.takeImage()
if image_data == []:
QR_FOUND = False
final_dist = dist_target
elif QR_FOUND:
dist_target = IT.getQRdist(image_data) #cm
theta = IT.getAngle(image_data)
print("theta1 = ", theta)
Motors.turnDegrees(theta)
time.sleep(1)
time.sleep(2)
print("QR FOUND from angle = ", QR_FOUND)
return (QR_FOUND, dist_target)
if phi > 45 or phi < -45: # Break turn into 'inc' increments so that the tracks dont fall off
turns = np.array([phi / inc for i in range(inc)])
print(turns)
else:
turns = np.array([phi])
for i in range(len(turns)):
turn = turns[i].item()
Motors.turnDegrees(turn, speed=5)
time.sleep(1)
image_data = IT.takeImage()
if image_data['barcode_len'] == 0:
dist_target = new_target_dist # Reinitialise the distance to the target
else:
dist_target = IT.getQRdist(image_data)
theta = IT.getAngle(image_data)
Motors.turnDegrees(theta)
orient = 0 # degrees -> Reinitialise the orientation 0 deg is facing the target
#Motors.write(mtr_speed, mtr_speed) # Get on your bike
target_time0 = timer(
) # Get time that the Jankanator starts moving towards the target again
target_dist0 = tacho2distance(
rc.readAttribute(OPTYPE.MOTOR_TACHO))
target_timer = True # Turn target timer on
print("Started moving toward target again: ", target_time0)
AVOIDING_TIMER = False # Re-initialise timer so that it can be restarted in the next avoiding stage
else: # If object has been avoided
#print("STILL NOT AVOIDING")
Motors.write(mtr_speed, mtr_speed) # Keep on trucking
from micromelon import *
from micromelon.comms_constants import MicromelonType as OPTYPE
import math
#from picamera.array import PiRGBArray
#from picamera import PiCamera
from timeit import default_timer as timer
import time
import cv2
import time
import numpy as np
import timeit
from pyzbar import pyzbar
import imageTester as IT
rc = RoverController()
rc.connectIP()
data = IT.takeImage()
dist = IT.getQRdist(data)
print("dist = ", dist)
angle = IT.getAngle(data)
print("theta = ", angle)
# phi = [360/8 for i in range(8)]
# print(phi)
# for i in phi:
# Motors.turnDegrees(i)
