[I]'Twas the night before ship day, And all through the shop, Not a creature was stirring, Not even[/I] -- Naah, they'll never believe that. - GaryVoshol [more]
 Chief Delphi Angle of bot in relation to peg
 CD-Media CD-Spy
 portal register members calendar search Today's Posts Mark Forums Read FAQ rules

#1
07-17-2017, 05:05 PM
 Adam_P Registered User FRC #3061 (Huskie Robotics) Team Role: Programmer Join Date: Jan 2017 Rookie Year: 2011 Location: Naperville, IL Posts: 36
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
07-17-2017, 05:19 PM
 mikets Software Engineer FRC #0492 (Titan Robotics Club) Team Role: Mentor Join Date: Jan 2010 Rookie Year: 2008 Location: Bellevue, WA Posts: 798
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```
Full repo can be accessed here.
https://github.com/trc492/Frc2017FirstSteamWorks
__________________
#3
07-17-2017, 11:35 PM
 GeeTwo Mentor AKA: Gus Michel II FRC #3946 (Tiger Robotics) Team Role: Mentor Join Date: Jan 2014 Rookie Year: 2013 Location: Slidell, LA Posts: 5,648
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.
__________________

If you can't find time to do it right, how are you going to find time to do it over?
If you don't pass it on, it never happened.
Robots are great, but inspiration is the reason we're here.
Friends don't let friends use master links.
#4
07-18-2017, 01:11 PM
 BenBernard Programming and Controls Mentor FRC #5687 (The Outliers) Team Role: Mentor Join Date: Jan 2016 Rookie Year: 2015 Location: Portland, ME Posts: 129
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
07-18-2017, 01:15 PM
 Rob Heslin Registered User no team Join Date: Feb 2017 Rookie Year: 2002 Location: Rochester, NY Posts: 30
Re: Angle of bot in relation to peg

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
07-18-2017, 01:35 PM
 Rob Heslin Registered User no team Join Date: Feb 2017 Rookie Year: 2002 Location: Rochester, NY Posts: 30
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
07-18-2017, 07:07 PM
 MuskieProgramme Registered User FRC #6420 (Fire Island Robotics) Team Role: Alumni Join Date: Dec 2016 Rookie Year: 2014 Location: Muscatine, IA Posts: 305
Re: Angle of bot in relation to peg

Quote:
 Originally Posted by BenBernard 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.
Actually, you would need to know a bit more than that. Such as were, precisely, your robot is. Which is rather difficult -- it can be done, but what if a wheel slips? What if there's a mechanical failure in your drive train? What if your robot was set up slightly wrong? That's when vision is useful.
__________________
#8
07-18-2017, 09:56 PM
 Rob Heslin Registered User no team Join Date: Feb 2017 Rookie Year: 2002 Location: Rochester, NY Posts: 30
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
07-19-2017, 07:53 PM
 BenBernard Programming and Controls Mentor FRC #5687 (The Outliers) Team Role: Mentor Join Date: Jan 2016 Rookie Year: 2015 Location: Portland, ME Posts: 129
Re: Angle of bot in relation to peg

Quote:
 Originally Posted by MuskieProgramme Such as were, precisely, your robot is.
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 field-relative 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 peg--since 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 math--that's available in countless CD threads and in many teams' open-sourced vision code. Our function for that (getAngle) is available in our github.

One of the best resources on vision is 254's intro.
#10
07-19-2017, 09:50 PM
 mikets Software Engineer FRC #0492 (Titan Robotics Club) Team Role: Mentor Join Date: Jan 2010 Rookie Year: 2008 Location: Bellevue, WA Posts: 798
Re: Angle of bot in relation to peg

Quote:
 Originally Posted by BenBernard 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 peg--since few camera have a FOV that would allow them to see an object at 66deg or 126deg.
This is not quite true. If you want to check the angle of the peg relative to the robot, chances are you are not checking it at your initial starting position. Your robot probably has moved forward a distance and trying to figure out how much you should turn to "face the peg" after that. In theory, if you parked your robot precisely at a determined position and the robot precisely moved forward some distance, you could calculate the angle you have to turn. Doesn't even need the camera. However, it is almost impossible to park the robot precisely at a pre-determined position every time. In addition, sensors such as encoders do have errors (e.g. wheel slipping... etc) and also your PID control or Motion profile drive will not quite precisely stop at the distance you would like. So there are all these cumulative errors that your calculation has to deal with. Vision feedback is a way to make these errors not matter too much.
__________________
#11
08-10-2017, 10:58 AM
 wlogeais Registered User FRC #2177 (The Robettes) Team Role: Mentor Join Date: Feb 2016 Rookie Year: 2011 Location: Minnesota Posts: 62
Re: Angle of bot in relation to peg

I am a bit late here, a nice long CD-break since Champs...
But this still seems to be a very relevant topic as teams prepare for Fall-off-season events.

I certainly agree with BenBernard back in post#4, it is far easier to determine parallel vs spin-left/spin-right 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 teleOP-vision 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 Vision-FOV 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 head-on, vs left-or-right and needing to strafe.

==> for this you already need to know your gyro-approach offset (post 4 again...), and calculate where the target-center is vs where it should be.
#12
08-10-2017, 02:36 PM
 jheidegger Registered User AKA: Jonathan Heidegger FRC #3176 (Purple Precision) Team Role: Programmer Join Date: Jan 2016 Rookie Year: 2014 Location: Brownsburg Posts: 14
Re: Angle of bot in relation to peg

Quote:
 Originally Posted by BenBernard 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.
Our team did a similar strategy to this and had a look up table that found the nearest peg angle. Then would then apply the vision algorithm with the robot head on the to target which increases the reliability of our vision algorithm because we did not have to account to the angle. The angle was obtained from a gyro and checked against the predicted angle of the robot from our swerve kinematics. All that was needed was to add a rough calculation for which way to move while turning to be flush with the peg. This would make sure we would keep locked on the Target and not loose it as the control loop turned the robot. All in all it was effective but hastily written in the pits and could be improved greatly.

 Thread Tools Display Modes Rate This Thread Linear Mode Rate This Thread: 5 : Excellent 4 : Good 3 : Average 2 : Bad 1 : Terrible

 Posting Rules You may not post new threads You may not post replies You may not post attachments You may not edit your posts vB code is On Smilies are On [IMG] code is On HTML code is Off
 Forum Jump User Control Panel Private Messages Subscriptions Who's Online Search Forums Forums Home Announcements     User Announcements FIRST     General Forum         FIRST E-Mail Blast Archive     Rumor Mill     Career     Robot Showcase Technical     Technical Discussion     Robotics Education and Curriculum     Motors     Electrical         CAN     Programming         NI LabVIEW         C/C++         Java         Python     Control System         FRC Control System         Sensors     Pneumatics     Kit & Additional Hardware     CAD         Inventor         SolidWorks         Creo     IT / Communications         3D Animation and Competition         Website Design/Showcase         Videography and Photography         Computer Graphics     National Instruments LabVIEW and Data Acquisition         LabView and Data Acquisition Competition     Unsung FIRST Heroes     Awards         Chairman's Award     Rules/Strategy         Scouting         You Make The Call     Team Organization         Fundraising         Starting New Teams         Finding A Team         College Teams     Championship Event     Regional Competitions     District Events     Off-Season Events     Thanks and/or Congrats     FRC Game Design     OCCRA         OCCRA Q&A         OCCRA Programming Other     Chit-Chat         Games/Trivia             Fantasy FIRST     Car Nack's Corner     College & University Education     Dean Kamen's Inventions     FIRST-related Organizations         Western Region Robotics Forum         Southern California Regional Robotics Forum         The Blue Alliance             Video Archives     FIRST In the News...     FIRST Lego League         Lego Mindstorm Discussion     FIRST Tech Challenge     VEX         VEX Robotics Competition         VEX IQ     Televised Robotics     Math and Science         NASA Discussion ChiefDelphi.com Website     CD Forum Support     Extra Discussion

All times are GMT -5. The time now is 12:41 AM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.

 -- English (12 hour) -- English (24 hour) Contact Us - Chief Delphi - Rules - Archive - Top