

FIRST has always been less than just a school activity for me and more of a way of everyday life.  Amanda Morrison [more] 



Thread Tools  Rate Thread  Display Modes 
#1




Angle of bot in relation to peg
Hey!
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. Thanks 
#2




Re: Angle of bot in relation to peg
Our code has detail comments on the math:
Code:
public TargetInfo getTargetInfo() { TargetInfo targetInfo = null; Rect targetRect = getTargetRect(); if (targetRect != null) { // // Physical target width: W = 10 inches. // Physical target distance 1: D1 = 20 inches. // Target pixel width at 20 inches: w1 = 115 // Physical target distance 2: D2 = 24 inches // Target pixel width at 24 inches: w2 = 96 // Camera lens focal length: f // W/D1 = w1/f and W/D2 = w2/f // => f = w1*D1/W and f = w2*D2/W // => w1*D1/W = w2*D2/W // => w1*D1 = w2*D2 = PIXY_DISTANCE_SCALE = 2300 // // Screen center X: Xs = 320/2 = 160 // Target center X: Xt // Heading error: e = Xt  Xs // Turn angle: a // tan(a) = e/f // => a = atan(e/f) and f = w1*D1/W // => a = atan((e*W)/(w1*D1)) // double targetCenterX = targetRect.x + targetRect.width/2.0; double targetXDistance = (targetCenterX  RobotInfo.PIXYCAM_WIDTH/2.0)*TARGET_WIDTH_INCHES/targetRect.width; double targetYDistance = PIXY_DISTANCE_SCALE/targetRect.width; double targetAngle = Math.toDegrees(Math.atan(targetXDistance/targetYDistance)); targetInfo = new TargetInfo(targetRect, targetXDistance, targetYDistance, targetAngle); if (debugEnabled) { robot.tracer.traceInfo( moduleName, "###TargetInfo###: xDist=%.1f, yDist=%.1f, angle=%.1f", targetXDistance, targetYDistance, targetAngle); } } if (targetFoundLED != null) { targetFoundLED.setState(targetInfo != null); } if (targetAlignedLED != null) { targetAlignedLED.setState(targetInfo != null && Math.abs(targetInfo.angle) <= 2.0); } return targetInfo; } //getTargetInfo https://github.com/trc492/Frc2017FirstSteamWorks 
#3




Re: Angle of bot in relation to peg
The key thing to measure is the aspect ratio of the reflective area of the tape. If you are square with the target, it will have a ratio similar to the published ratio. As you move to either side, the width will decrease relative to the height. If you're a good bit off to one side, you can figure out whether you're to the right or left by checking which side of the target is larger (presuming a basically rectangular target area)  the larger side is the nearer side.

#4




Re: Angle of bot in relation to peg
You shouldn't need to us vision to determine the robot's angle relative to the spring. As long as you know WHICH spring you are near, you should be able to calculate the angle from a gyro. Even if you don't know which spring you are near, there are only 3 choices, so you should be able to determine it from your image and the your gyro.

#5




Re: Angle of bot in relation to peg
This is my best answer.
If you have used the target's height to computer distance to the target and are looking at the target mostly head on, you should begin by setting up a triangle with the target of width W as one side and the camera as the corner opposite. The angle between the two sides that enter the camera corner is known to you, and can be computed, we can call it A. Draw the distance to the target(D) as a line from the midpoint of the width side to camera corner. You are looking for the angle between this line and the side of length W, we'll call this angle Theta, which is our goal to find. Setup three equations, using the Law of Cosines for each of the three triangles you have made. Though we don't know the lengths of the two outside legs of the largest triangle, we don't need to. You should have three equations three unknowns. Solve for theta, eliminating the two leg lengths. The result I get is: Theta=ACOS(SQRT([(D/W)+(W/(4D))]^2[{(D/W)(W/(4D))}/cos(A)]^2)) As to which side of the peg you are on, GeeToo has a good point. 
#6




Re: Angle of bot in relation to peg
Not sure exactly what you are looking for, if you are looking for the angle to the peg this is my best answer.
If you have used the target's height to computer distance to the target and are looking at the target mostly head on, you should begin by setting up a triangle with the target of width W as one side and the camera as the corner opposite. The angle between the two sides that enter the camera corner is known to you, and can be computed, we can call it A. Draw the distance to the target(D) as a line from the midpoint of the width side to camera corner. You are looking for the angle between this line and the side of length W, we'll call this angle theta, which is our goal. Setup three equations, using the Law of Cosines for each of the three triangles you have made. Though we don't know the lengths of the two outside legs of the largest triangle, we don't need to. You should have three equations three unknowns. Solve for theta, eliminating the two leg lengths. The result I get is: theta=ACOS(SQRT([(D/W)+(W/(4D))]^2[{(D/W)(W/(4D))}/cos(A)]^2)) As to which side of the peg you are on, GeeToo has a good point. With two separate particles, the taller is going to be the closer side. 
#7




Re: Angle of bot in relation to peg
Quote:

#8




Re: Angle of bot in relation to peg
Not sure exactly what you are looking for, if you are looking for the angle to the peg this is my best answer.
If you have used the target's height to computer distance to the target and are looking at the target mostly head on, you should begin by setting up a triangle with the target of width W as one side and the camera as the corner opposite that side. The angle between the two sides that enter the camera corner is known to you, and can be computed, we can call it A. Draw the distance to the target(D) as a line from the midpoint of the width side to camera corner. You are looking for the angle between this line and the side of length W, we'll call this angle theta, which is our goal. Setup three equations, using the Law of Cosines for each of the three triangles you have made. Though we don't know the lengths of the two outside legs of the largest triangle, we don't need to. You should have three equations three unknowns. Solve for theta, eliminating the two leg lengths. The result I get is: theta=ACOS(SQRT([(D/W)+(W/(4D))]^2[{(D/W)(W/(4D))}/cos(A)]^2)) As to which side of the peg you are on, GeeToo has a good point, with targets this year the closer one will be bigger. 
#9




Re: Angle of bot in relation to peg
I read the original request as looking for the angle between the centerline of the robot and the centerline of the peg, for which you need to know your robot's fieldrelative angle, but not where precisely it is. If your gyro is telling you you are at 66deg relative to your starting position (which in this game is most likely parallel with the field) then you know you are at 6deg relative to the left peg, 66deg relative to the center peg, and 126% relative to the right peg. If you can SEE a peg, then odds are very high it's the left pegsince few camera have a FOV that would allow them to see an object at 66deg or 126deg.
Now, if the OP was not looking for the robot's angle relative to the peg, but rather the offset angle of the center of the vision targets from the robot, that's a different set of maththat's available in countless CD threads and in many teams' opensourced vision code. Our function for that (getAngle) is available in our github. One of the best resources on vision is 254's intro. 
#10




Re: Angle of bot in relation to peg
Quote:

#11




Re: Angle of bot in relation to peg
I am a bit late here, a nice long CDbreak since Champs...
But this still seems to be a very relevant topic as teams prepare for Falloffseason events. I certainly agree with BenBernard back in post#4, it is far easier to determine parallel vs spinleft/spinright based on a predetermined PEG/GYRO difference. Yes this is absolutely true, the auton() needs to know which side its approaching, and then it WILL know if it is parallel or not (without any vision logic). BTW, this awareness can be built into teleOPvision logic too. The other big challenges remain, I think the latter below is exactly mikets point about the where vision adds the most value. 1. How close are is the airship? ==> Use VisionFOV distance calcs. However as you get close you may need to 'target' the gap between contours rather than contour heights, in case the contours are slipping outside of the image/frame. ==> Or you can look into the tinyLIDAR or equivalent options. 2. Are we approaching headon, vs leftorright and needing to strafe. ==> for this you already need to know your gyroapproach offset (post 4 again...), and calculate where the targetcenter is vs where it should be. 
#12




Re: Angle of bot in relation to peg
Quote:

Thread Tools  
Display Modes  Rate This Thread 

