Angle of bot in relation to peg
I'm working on a vision project for my team and am using last year's Steamworks challenge to start. I've hit a roadblock regarding finding the angle my robot is in relation to the gear. I've been able to calculate the distance I am from the peg as well as the lateral distance I am from it, but I am not able to figure out the angle accurately. Our team has swerve drive so knowing the angle matters and we should be able to correct based on that.