def anti_turn(): h = get_imu_head() n = get_imu_head() if h>90: while abs(h-n)<90: anticlockwise() n =get_imu_head() else: while n>=0: anticlockwise() n=get_imu_head() while (360+h-n)<90: anticlockwise() n=get_imu_head()
def clock_turn():
h = get_imu_head()
z = 0
while z - h < 90:
n = get_imu_head()
if n < h:
z = 360 + n
else:
z = n
clockwise1()
val = getball()
if (val == "FOUND"):
brute_stop()
print(val, "Ball detected")
quit()
def main():
pg.init()
screen = pg.display.set_mode((300, 300))
clock = pg.time.Clock()
player_img = pg.Surface((42, 70), pg.SRCALPHA)
pg.draw.polygon(player_img, pg.Color('white'), [(0, 70), (21, 2),
(42, 70)])
player_rect = player_img.get_rect(center=screen.get_rect().center)
while True:
for event in pg.event.get():
if event.type == pg.QUIT:
return
adjusted_angle = get_imu_head()
pg.display.set_caption('angle {:.2f}'.format(adjusted_angle))
# Rotate the image and get a new rect.
player_rotated = pg.transform.rotozoom(player_img, -adjusted_angle, 1)
player_rect = player_rotated.get_rect(center=player_rect.center)
screen.fill((30, 30, 30))
screen.blit(player_rotated, player_rect)
pg.display.flip()
clock.tick(60)
def match_head():
waypoint_heading,dist=get_heading()
while True:
#waypoint_heading_opp=waypoint_heading+180
imu_heading=get_imu_head()
heading_diff=imu_heading-waypoint_heading
print(imu_heading,waypoint_heading,heading_diff)
#detect()
if imu_heading < waypoint_heading+20 and imu_heading>waypoint_heading-20:
#brute_stop()
break
if heading_diff >=-180:
if heading_diff<=0:
clockwise()
if heading_diff <-180:
anticlockwise()
if heading_diff>=0:
if heading_diff<180:
turn = 2
anticlockwise()
if heading_diff >= 180:
turn = 1
clockwise()
