Automatic Alignment

[CarlyAmar]

We have 2 long range IR's and the pixycams to do this

[CarlyAmar]

We need this to do #178 : Autonomous for picking up crates and we may also use this in teleop attached to a button

[CarlyAmar]

See: #126

[calvin-godfrey]

So I graphed how the ratio of width/height changes based on the angle.

and the python code I used to make this is:

import matplotlib
from math import sqrt
matplotlib.use('Agg')
import matplotlib.pyplot as plt
width = 26.9
length = 16.9
widthChange = 26.9/90
lengthChange = 16.9/90
startCoords = [0.0, 0.0]
endCoords = [26.9, 0.0]
ratio = []
angle = [x for x in range(1, 91)]
def distance(x1, y1, x2, y2):
    return sqrt((x1-x2)**2+(y1-y2)**2)
for i in range(90):
    d = distance(startCoords[0], startCoords[1], endCoords[0], endCoords[1])
    ratio.append(d/12.1) #height
    startCoords[0] += widthChange
    endCoords[1] += lengthChange
for i in ratio:
    print "(" + str(angle[ratio.index(i)]) + "," + str(i) + ")"

[calvin-godfrey]

This doesn't completely work. I'm doing some testing and I think there's some cases where it returns a nonreal answer which will return an error.