How to more accurately calculate angle to goal with offsets?

[team-4480]

Hi!

We were excited to finally have an angle to goal PID loop somewhat working the other day until we realized how inconsistent it was. Sometimes it would be dead on, other times it would be just enough off that our shot missed. I set the tolerance to a minimal .5 degrees so it should be going right where it is told to be going. The prime suspect is how our camera and gyro is mounted.(C is for camera and G is for gyro, arrow points to where we line up)

We have never done vision before this season so we weren't exactly trained on where the ideal places are for the camera and gyro to make programming easier. Thankfully for programming, we do shoot only a couple feet from the end of the batter, so there is more wiggle room. Our current vision code looks like this:

###X is inputted into gyroMagic which then is used for when I call the visionTurn function for the PID loop###

    def gyroMagic(self, x):
        angle = ((x-170)*(60/320)) #170 is about center of target when lined up
        yaw=self.navx.getYaw()
        print (yaw)
        if yaw > 0:
            self.desiredAngle=self.navx.getYaw()+(angle) 
            if self.desiredAngle>179:
                self.desiredAngle -= 360
        else:
            self.desiredAngle=self.navx.getYaw()+(angle)
            if self.desiredAngle<-179:
                self.desiredAngle += 360

        return self.desiredAngle

    def visionTurn(self):

        if self.vision_state == 1:
            self.turnController.setSetpoint(self.desiredAngle)
            self.rotateReady = True
            self.vision_state=2
        elif self.vision_state == 2:

            if self.turnController.onTarget() or self.cancel.get():
                self.vision_state=3
                self.rotateReady=False
                self.auto_aline_autoNow=False

So my ultimate question is, as someone who hasn't taken a trig class yet, how do I offset the camera and gyro placement to get a more accurate angle to the target? I am also of thinking of changing my angle algorithm to copy what Jared Russell recommends, but I believe that an offset algorithm would make our robot shoot much better. Any help would be greatly appreciated! Thanks!

[Greg Woelki]

Originally Posted by team-4480

Hi!

We were excited to finally have an angle to goal PID loop somewhat working the other day until we realized how inconsistent it was. Sometimes it would be dead on, other times it would be just enough off that our shot missed. I set the tolerance to a minimal .5 degrees so it should be going right where it is told to be going. The prime suspect is how our camera and gyro is mounted.(C is for camera and G is for gyro, arrow points to where we line up)

We have never done vision before this season so we weren't exactly trained on where the ideal places are for the camera and gyro to make programming easier. Thankfully for programming, we do shoot only a couple feet from the end of the batter, so there is more wiggle room. Our current vision code looks like this:

###X is inputted into gyroMagic which then is used for when I call the visionTurn function for the PID loop###

    def gyroMagic(self, x):
        angle = ((x-170)*(60/320)) #170 is about center of target when lined up
        yaw=self.navx.getYaw()
        print (yaw)
        if yaw > 0:
            self.desiredAngle=self.navx.getYaw()+(angle) 
            if self.desiredAngle>179:
                self.desiredAngle -= 360
        else:
            self.desiredAngle=self.navx.getYaw()+(angle)
            if self.desiredAngle<-179:
                self.desiredAngle += 360

        return self.desiredAngle

    def visionTurn(self):

        if self.vision_state == 1:
            self.turnController.setSetpoint(self.desiredAngle)
            self.rotateReady = True
            self.vision_state=2
        elif self.vision_state == 2:

            if self.turnController.onTarget() or self.cancel.get():
                self.vision_state=3
                self.rotateReady=False
                self.auto_aline_autoNow=False

So my ultimate question is, as someone who hasn't taken a trig class yet, how do I offset the camera and gyro placement to get a more accurate angle to the target? I am also of thinking of changing my angle algorithm to copy what Jared Russell recommends, but I believe that an offset algorithm would make our robot shoot much better. Any help would be greatly appreciated! Thanks!

Regarding creating an offset curve:

Our camera is mounted approximately 8 inches off of our robot's center line. While you could use trig to create an offset curve, we instead chose to create an offset curve empirically as part of our camera calibration process. We moved the robot backwards in 2 foot increments from 4 to 20 feet away from our target mockup and, at each increment, turned the robot until it shot dead center into the target and recorded how far "off of alignment" our vision code said we were. With this information, we performed a simple curve fit in excel to create an equation for angle offset as a function of distance

We elected to use this method instead of straight math because empirical calibration also compensates for any misalignment that might exist between the pointing of the camera and the pointing of the shooter.



On the other hand, from the information you provided, the gyro and camera mounting aren't necessarily the causes of your issue. The gyro's off-centered mounting shouldn't be a problem because all points on a rigid body rotate with the same angular velocity. Our gyro is in the far back right-hand corner of our robot and we have had no issues with its location. The gyro shouldn't be causing a problem unless it is either mounted loosely so that it can shift around or if it is drifting enough to matter in the couple of seconds that it takes to auto align. On auto align timescales, neither the NavX nor the Spartan Board should drift significantly, but no matter what your team is using for a gyro, it wouldn't hurt to take a look at how quickly it drifts so you can rule out that possibility. The off-centered mounting of the camera could be causing an issue if the robot isn't consistently shooting from the same distance away from the target (my understanding of your post was that you're shooting from the same distance every time, is that correct?) If you're only shooting from one distance, you only need one offset, but that offset would be incorrect if the robot is actually shooting from a different distance. However, if the variation in distance is measured in inches rather than feet, that most likely isn't the issue.

Correct me if I'm wrong, but it seems like you are using the angle measurement from only a single image to determine the setpoint of your turning control loop. If this is the case, then the problem that Jared pointed out in the post you linked to would cause an issue. Have you noticed a difference in the accuracy of aligning from starting close to the correct angle versus starting far from the correct angle? If there is a difference, the simplest solution would be to just implement his formula. Another solution would be to change your alignment procedures so that the control loop setpoint gets updated based on new data every time a new picture is processed. This "video style" method has the advantage that the robot keeps double-checking where it thinks the goal is, so among other things (including eliminating the effects of any gyro drift that might - but really shouldn't - be occurring) it wouldn't really matter whether you use Jared's more accurate equation or the simple linear one. Ideally both of these solutions would be implemented, but the second one is more complicated, particularly if you end up needing to be clever about latency management.

Another possibility is that your vision code is working just fine but your control loop is not. Under what condition(s) does your turning PID loop terminate? When we were first working on our alignment loop, we noticed the significant "spring back" that occurs with our pneumatic wheels when we turn and then cut power. This phenomenon exists with all wheels, but is just more noticeable with flexible ones like the Andymark pneumatic tires. Per the suggestion of Austin Schuh, we keep our alignment control loop running until after we shoot so that the I term in one of our nested PID loops holds us at the correct angle against the restoring forces that would cause our drive train to spring back slightly. In order to tell whether your PID loop is working properly (whether premature termination of the loop is the issue or something else is), you should display angular error on the dashboard to confirm that the loop is doing exactly what it is supposed to be doing. I certainly am not an expert on control loops, but there are plenty of people on CD who are, so posting information about that part of your alignment procedures may prove fruitful.