[Snow Pickles]

I'm still not sure of this is going to work, it's going to need good camera tracking and accurate distance judging.
But if your still interested, attached is a drawing to try to explain the equation.

At point k the camera finds a range ( s ) and angle (Q) (relative to the heading of the robot). Then it moves M and takes another range of ss. From this the velocity and heading of the enemy robot can be calculated using the law of sines and cosines (hence all the trigness). All that was the ak_pp_kk stuff. In the code angle are apoint_point_point and lines are lpoint_point.

The next part is where things get crazy

Next I made the equation for the line T_ppp and then substituted that into that simple kinematic equations to come out with an equation with the Initial velocity to shoot at.
The problem was that it was 3 pages long and the velocity was all over the place. So I had to use Newton's Linerization to solve it (the same way graphing calculators find the interception of two lines). I had to take the derivative of this huge function and it got really big. This part is the whole while loop. For newton's method you plug in a guess (here it's the ss, which is'nt right, this needs to be fixed) and then use the answer as a second guess. When each number it returns becomes so close (in this case it's randomly 5) you have a good estimate. It should take less than 10 times, likely less to become very accurate.
Then from this it gets the best angle the turret should be at and then finds the angle the enemy is coming at you and returns these and the velocity in a set of pairs.
Looking back at this it is missing some stuff like a specification of accuracy, a pitch of the angle, and others. I'm fixing the code now and making it more understandable. I might also later make a pitch varying version rather than velocity.
Also, I will be using some of this to make a robot avoiding programming that you might see posted later on if I do finish it.