# Help with Vision Algorithm

Hi,

I am attempting to make an auto line up algorithm with our vision stats. Right now, I am just using the X and Y coordinates to do line up the forward/ back and the side to side of the robot. The problem is that the robot overcompensates most of the time, even when I have a generous range of about 30 pixels(in a 320 x 240 image). I read a lot about how to set up the vision processing, but I really haven’t seen much on actually using those values(probably because everyone’s robot is different). The summary of the code looks like this:

``````

visionXvalues = NetworkTables.getArray('centerX', self.centerX)
visionYvalues = NetworkTables.getArray('centerY', self.centerY)

if visionXvalues > 180:
TURN
elif visionXvalues < 120:
TURN
else:
DO NOTHING

if visionYvalues > 110:
GO FORWARD
elif visionYvalues < 80:
GO BACK
else:
DO NOTHING

``````

For the turning and such it is merely just me doing “self.motor.set(.25)”. My question is, is there a good way of quickly being able to line yourself up? I feel like the string of “if” statements are very clunky. Our robot shoots from the same relative area each time so I don’t need to mess with Trig stuff.

One of the possible solutions I thought we might try is using PID control, but I haven’t ever used it. Can you use PID only sometimes like when the align function is called? Or do you have to then implement it in all areas where you call that motor?

Also, I use GRIP if that changes anything.

Thanks!

There’s a bunch of ways to go about this, but the basic idea is to define some set of controls that allow you to ‘tell the robot to point at a particular direction’. Once you have that, you can:

• Determine the robot’s current angle (using a Gyro/NavX)
• Determine the offset angle from your target (via GRIP/whatever)
• Tell the robot to go to (current angle + offset)

For an example of using PIDController + NavX in pyfrc simulation to ‘go to a particular angle’, see this example: https://github.com/robotpy/robotpy-wpilib-utilities/tree/master/samples/navx_rotate_to_angle