OCCRA
Go to Post Parallax is evil. - RoboChair [more]
Home
Go Back   Chief Delphi > Technical > Programming
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 07-17-2017, 05:05 PM
Adam_P's Avatar
Adam_P Adam_P is offline
Registered User
FRC #3061 (Huskie Robotics)
Team Role: Programmer
 
Join Date: Jan 2017
Rookie Year: 2011
Location: Naperville, IL
Posts: 20
Adam_P is a jewel in the roughAdam_P is a jewel in the roughAdam_P is a jewel in the rough
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
Reply With Quote
  #2   Spotlight this post!  
Unread 07-17-2017, 05:19 PM
mikets's Avatar
mikets mikets is offline
Software Engineer
FRC #0492 (Titan Robotics Club)
Team Role: Mentor
 
Join Date: Jan 2010
Rookie Year: 2008
Location: Bellevue, WA
Posts: 768
mikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of light
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
__________________
Reply With Quote
  #3   Spotlight this post!  
Unread 07-17-2017, 11:35 PM
GeeTwo's Avatar
GeeTwo GeeTwo is offline
Technical Director
AKA: Gus Michel II
FRC #3946 (Tiger Robotics)
Team Role: Mentor
 
Join Date: Jan 2014
Rookie Year: 2013
Location: Slidell, LA
Posts: 4,641
GeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond reputeGeeTwo has a reputation beyond repute
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.
Reply With Quote
  #4   Spotlight this post!  
Unread 07-18-2017, 01:11 PM
BenBernard's Avatar
BenBernard BenBernard is offline
Programming and Controls Mentor
FRC #5687 (The Outliers)
Team Role: Mentor
 
Join Date: Jan 2016
Rookie Year: 2015
Location: Portland, ME
Posts: 77
BenBernard will become famous soon enough
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.
Reply With Quote
  #5   Spotlight this post!  
Unread 07-18-2017, 01:15 PM
Rob Heslin Rob Heslin is offline
Registered User
no team
 
Join Date: Feb 2017
Rookie Year: 2002
Location: Rochester, NY
Posts: 15
Rob Heslin is on a distinguished road
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.
Reply With Quote
  #6   Spotlight this post!  
Unread 07-18-2017, 01:35 PM
Rob Heslin Rob Heslin is offline
Registered User
no team
 
Join Date: Feb 2017
Rookie Year: 2002
Location: Rochester, NY
Posts: 15
Rob Heslin is on a distinguished road
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.
Reply With Quote
  #7   Spotlight this post!  
Unread 07-18-2017, 07:07 PM
MuskieProgramme MuskieProgramme is offline
Registered User
FRC #6420 (Fire Island Robotics)
Team Role: Alumni
 
Join Date: Dec 2016
Rookie Year: 2014
Location: Muscatine, IA
Posts: 149
MuskieProgramme will become famous soon enough
Re: Angle of bot in relation to peg

Quote:
Originally Posted by BenBernard View Post
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.
__________________
Reply With Quote
  #8   Spotlight this post!  
Unread 07-18-2017, 09:56 PM
Rob Heslin Rob Heslin is offline
Registered User
no team
 
Join Date: Feb 2017
Rookie Year: 2002
Location: Rochester, NY
Posts: 15
Rob Heslin is on a distinguished road
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.
Reply With Quote
  #9   Spotlight this post!  
Unread 07-19-2017, 07:53 PM
BenBernard's Avatar
BenBernard BenBernard is offline
Programming and Controls Mentor
FRC #5687 (The Outliers)
Team Role: Mentor
 
Join Date: Jan 2016
Rookie Year: 2015
Location: Portland, ME
Posts: 77
BenBernard will become famous soon enough
Re: Angle of bot in relation to peg

Quote:
Originally Posted by MuskieProgramme View Post
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.
Reply With Quote
  #10   Spotlight this post!  
Unread 07-19-2017, 09:50 PM
mikets's Avatar
mikets mikets is offline
Software Engineer
FRC #0492 (Titan Robotics Club)
Team Role: Mentor
 
Join Date: Jan 2010
Rookie Year: 2008
Location: Bellevue, WA
Posts: 768
mikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of light
Re: Angle of bot in relation to peg

Quote:
Originally Posted by BenBernard View Post
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.
__________________
Reply With Quote
  #11   Spotlight this post!  
Unread 08-10-2017, 10:58 AM
wlogeais wlogeais is offline
Registered User
FRC #2177 (The Robettes)
Team Role: Mentor
 
Join Date: Feb 2016
Rookie Year: 2011
Location: Minnesota
Posts: 37
wlogeais is an unknown quantity at this point
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.
Reply With Quote
  #12   Spotlight this post!  
Unread 08-10-2017, 02:36 PM
jheidegger jheidegger is offline
Registered User
AKA: Jonathan Heidegger
FRC #3176 (Purple Precision)
Team Role: Programmer
 
Join Date: Jan 2016
Rookie Year: 2014
Location: Brownsburg
Posts: 14
jheidegger is an unknown quantity at this point
Re: Angle of bot in relation to peg

Quote:
Originally Posted by BenBernard View Post
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.
Reply With Quote
Reply


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

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


All times are GMT -5. The time now is 10:42 PM.

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


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi