What is wrong with my Vision to Gyro code?!

[team-4480]

Hi,

I have been trying to make a decent attempt at vision tracking for offseason competitions. During regional play, I would just do "if target to the left, go right" which is incredibly slow and just a bad practice.

Now I am trying to use the NavX so that it will be quicker and better at aligning with the target. I wrote some code to do this by first finding the best target. Then it will calculate the gyro angle before finally having that gyro angle put into a PID loop.

The problem I am having is that the PID loop is not going to where it should go. For example, I printed out the current Yaw angle before going to the target, the target angle, and then the yaw angle as it moves. What I found is that the target is -56 degrees, the current Yaw angle before moving is -70, and where the robot turns to is about 125 and it just oscillates there. This confused the heck out of me because I am telling the PID loop to go to -56 but then is stops at 125?! Why?!

Here is the code(my apologies in advance for the spaghetti code):

    def vision(self):

        try:
            self.vision_table.retrieveValue('centerX', self.vision_x)
            self.vision_table.retrieveValue('centerY', self.vision_y)
        except KeyError:
            self.turner=False
        else:
            if len(self.vision_x)>0 and self.auto_alineX.get() and self.vision_state == 3 or self.auto_aline_auto:
                self.degrees=self.gyroMagic(self.find_bestX())
                self.vision_state = 1

        self.visionTurn()

    def find_bestX(self):
        #My attempt to find the best target by seeing which one is more in the center
        if len(self.vision_x)==1:
            self.vision_numberX=self.vision_x[0]
        else:
            good=self.vision_x[0]
            normal=abs(self.vision_x[0]-110)
            for i in self.vision_x[1:]:
                total=abs(i-110)
                if total <= normal:
                    normal=total
                    good=i
            self.vision_numberX=good
        return self.vision_numberX

    def gyroMagic(self, x):
        angle = ((x-110)*(60/320)) #110 seems to be about the center

        yaw=self.navx.getYaw()
        if yaw > 0:
            self.desiredAngle=self.navx.getYaw()+(angle*(227/180)) #Times 227/180 because the gyro is very offset
            if self.desiredAngle>179:
                self.desiredAngle -= 360
        else:
            self.desiredAngle=self.navx.getYaw()+(angle*(133/180))
            if self.desiredAngle<-179:
                self.desiredAngle += 360
        return self.desiredAngle

    def visionTurn(self):
        #Where the PID stuff happens
        #Used a state machine because it was an easy way to have the setSetpoint run only once
        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

I honestly don't know what I am doing wrong that the PID loop wants to go to the 125 degree point instead of -56 where I am telling it to go. Any help would be greatly appreciated! Thanks!

EDIT: I realized it may help if I show the setup of the PID loop. Here it is:

        kP = 0.03
        kI = 0.00
        kD = 0.00
        kF = 0.00
        turnController = wpilib.PIDController(kP, kI, kD, kF, self.navx, output=self)
        turnController.setInputRange(-180.0,  180.0)
        turnController.setOutputRange(-.5, .5)
        turnController.setAbsoluteTolerance(2.0)
        turnController.setContinuous(True)

        self.turnController = turnController

[pblankenbaker]

You indicated that your PID loop would rotate the robot to 125 degrees instead of the target of -56 degrees. Given that the robot then oscillates around 125 degrees, this sounds like a condition where you need to invert (negate) the output value produced by the PID before applying it to your motors.

When we run across this issue, we typically observe the following:

• Robot rotates away from target because we are applying power in the opposite direction required to reduce the error.
• As the robot rotates away from the target, the error increases and the power output increases (it typically speeds up as it "runs away" from the target).
• Because the PID set to continuous mode, 180 degrees is the farthest away from the target that you can get (so once it goes past that, the PID probably inverts the error on you and you bounce back hard in the other direction).

Try negating the power before applying it to your motors to see if that fixes the problem.

It might also be useful to add a command that rotates the robot 90 degrees every time you press a button (to make sure you get the rotation PID figured out before adding the camera code).

[ollien]

Originally Posted by pblankenbaker

You indicated that your PID loop would rotate the robot to 125 degrees instead of the target of -56 degrees. Given that the robot then oscillates around 125 degrees, this sounds like a condition where you need to invert (negate) the output value produced by the PID before applying it to your motors.

When we run across this issue, we typically observe the following:

• Robot rotates away from target because we are applying power in the opposite direction required to reduce the error.
• As the robot rotates away from the target, the error increases and the power output increases (it typically speeds up as it "runs away" from the target).
• Because the PID set to continuous mode, 180 degrees is the farthest away from the target that you can get (so once it goes past that, the PID probably inverts the error on you and you bounce back hard in the other direction).

Try negating the power before applying it to your motors to see if that fixes the problem.

It might also be useful to add a command that rotates the robot 90 degrees every time you press a button (to make sure you get the rotation PID figured out before adding the camera code).

My team ran into a similar issue when trying to use PID and a NavX to make our robot drive straight. What I was confused about (and still am, quite frankly) is as to how we would get a steady state error 180 degrees from the target. You said in your post that

Because the PID set to continuous mode, 180 degrees is the farthest away from the target that you can get...

If the error is still such a huge value, why wouldn't the PID loop account for that?

[Jaci]

Your PID Controller is steadying to a 180-degree offset of where you put your setpoint (i.e. the 'back-end' of your robot is facing your target), e.g. -56 + 180 = 124, within the 2 degree tolerance you set

Before passing your setpoint to the PID controller, use "setpoint - 180" instead of just "setpoint", or find a way to reverse the angle (angle - 180) the NavX passes to the PIDController

Try negating the power before applying it to your motors to see if that fixes the problem.

This won't work as it doesn't fix the value the NavX is reporting. PID is relative

[euhlmann]

Originally Posted by Jaci

This won't work as it doesn't fix the value the NavX is reporting. PID is relative

This doesn't make sense. OP said that the NavX reports 125 when the PID loop is set to -56. The PID loop uses the NavX input. This PID is based on an absolute yaw heading, so it's not relative as you say.

My point is, the NavX doesn't report 2 different things for the PID loop and SmartDashboard display. The operation (180 - angle) will fix the problem by changing the clockwise angles to counterclockwise ones, but OP probably doesn't want to do that because clockwise is the convention. Instead, it's a better idea to flip the output direction.

[Jaci]

Originally Posted by euhlmann

This doesn't make sense. OP said that the NavX reports 125 when the PID loop is set to -56. The PID loop uses the NavX input. This PID is based on an absolute yaw heading, so it's not relative as you say.

My point is, the NavX doesn't report 2 different things for the PID loop and SmartDashboard display. The operation (180 - angle) will fix the problem by changing the clockwise angles to counterclockwise ones, but OP probably doesn't want to do that because clockwise is the convention. Instead, it's a better idea to flip the output direction.

When I say 'relative', I am referring to the calculation of 'error' in PID (i.e. where the robot is RELATIVE to where it wants to be)

Flipping the output direction will not solve the problem, but instead cause the robot to steady by turning in the opposite direction.

The error is calculated by the difference between where the gyro is versus where it wants to be. By changing the output variables, this does not change this calculated error. Either the setpoint or the feedback given by the NavX needs to be inverted (that is, whatever it is minus 180 degrees).

'Offsetting' the setpoint based on what the real-world offset is will account for what seems like the NavX being placed backwards. Flipping the output directions, however, does not change the real-world offset, but instead causes the robot to rotate away from its target, which will then wrap around due to the continuous option being set, and steady to the same angle as before.

[euhlmann]

Originally Posted by Jaci

When I say 'relative', I am referring to the calculation of 'error' in PID (i.e. where the robot is RELATIVE to where it wants to be)

Flipping the output direction will not solve the problem, but instead cause the robot to steady by turning in the opposite direction.

The error is calculated by the difference between where the gyro is versus where it wants to be. By changing the output variables, this does not change this calculated error. Either the setpoint or the feedback given by the NavX needs to be inverted (that is, whatever it is minus 180 degrees).

'Offsetting' the setpoint based on what the real-world offset is will account for what seems like the NavX being placed backwards. Flipping the output directions, however, does not change the real-world offset, but instead causes the robot to rotate away from its target, which will then wrap around due to the continuous option being set, and steady to the same angle as before.

You understand that if the NavX says it's at -56 degrees, it's at -56 degrees. If the robot is supposed to go to 125 and the NavX says it's -56, there's something wrong with the PID loop. It's not "actually at 125 but reporting -56," it's at -56. It's not "flipped 180 degrees" because if your PID loop is working correctly, the NavX reading and setpoint need to match at the steady state. No amount of playing with the NavX value is going to make a PID loop that doesn't match its setpoint go to its setpoint. You need to change the PID loop.

Sorry if I'm not being clear with this. Does this make sense?

[Jaci]

Originally Posted by euhlmann

You understand that if the NavX says it's at -56 degrees, it's at -56 degrees. If the robot is supposed to go to 125 and the NavX says it's -56, there's something wrong with the PID loop. It's not "actually at 125 but reporting -56," it's at -56. It's not "flipped 180 degrees" because if your PID loop is working correctly, the NavX reading and setpoint need to match at the steady state. No amount of playing with the NavX value is going to make a PID loop that doesn't match its setpoint go to its setpoint. You need to change the PID loop.

Sorry if I'm not being clear with this. Does this make sense?

The way I understand the question is that OP sets the setpoint to -56 degrees, and then the robot rotates to a value that looks, to them, as 125 degrees. Given that the PID loop is WPILib's built-in, fiddling with the internals aren't going to solve any issues. The value being passed to the loop needs to change. Again, changing the wheel outputs aren't going to change the resting position of the loop.

OP: I ran your python code using the values you provided. Here's what I found:
In your gyroMagic function, angle and desiredAngle are both always zero. You're trying to divide an integer type by an integer. Use the float notation for the numbers you're dividing by (e.g. 110.0, 60.0, 227.0 etc).

Where are you plugging your -56 into?

[team-4480]

Originally Posted by Jaci

OP: I ran your python code using the values you provided. Here's what I found:
In your gyroMagic function, angle and desiredAngle are both always zero. You're trying to divide an integer type by an integer. Use the float notation for the numbers you're dividing by (e.g. 110.0, 60.0, 227.0 etc).

Where are you plugging your -56 into?

You must have tested it using Python 2.7.X. I tried my angle equation in Python 2 and got 0 while Python 3.5.1 gave me 18.75 when using an example x value of 210. Conveniently, RobotPy uses Python 3.

I am using the -56 for the visionTurn() function which is where the setSetpoint is located.

Thanks!

[Jaci]

Originally Posted by team-4480

You must have tested it using Python 2.7.X. I tried my angle equation in Python 2 and got 0 while Python 3.5.1 gave me 18.75 when using an example x value of 210. Conveniently, RobotPy uses Python 3.

I am using the -56 for the visionTurn() function which is where the setSetpoint is located.

Thanks!

Are you reading the 125 degrees by eye, or from a print statement? If it's the latter, a code reference for where this statement is would be eternally useful

[team-4480]

Originally Posted by Jaci

Are you reading the 125 degrees by eye, or from a print statement? If it's the latter, a code reference for where this statement is would be eternally useful

It is indeed the latter

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:
          
            print (self.navx.getYaw())

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

[ollien]

Originally Posted by euhlmann

The operation (180 - angle) will fix the problem by changing the clockwise angles to counterclockwise ones, but OP probably doesn't want to do that because clockwise is the convention.

Isn't that what negating the PID output would do?

[GeeTwo]

Originally Posted by euhlmann

The operation (180 - angle) will fix the problem by changing the clockwise angles to counterclockwise ones, but OP probably doesn't want to do that because clockwise is the convention. Instead, it's a better idea to flip the output direction.

Originally Posted by ollien

Isn't that what negating the PID output would do?

While the camera will drive to the same location with both of these changes, the reasons and behavior will be different.

By reversing the angle, the physical orientation will move to the correct location, but it will continue to oscillate around that new point, for reasons given in post #2 - Even if it were started right at that physical point at rest, the PID is 180 degrees off target, and will push it one way or the other, starting an oscillation.

If the PID output is negated (or the motor wires swapped), the motor will then be driven towards the desired set point, and the PID should (after a bit of tuning) function as desired, either approaching the set point asymptotically or slightly overshooting with a dampening oscillation.

[ollien]

Originally Posted by GeeTwo

While the camera will drive to the same location with both of these changes, the reasons and behavior will be different.

By reversing the angle, the physical orientation will move to the correct location, but it will continue to oscillate around that new point, for reasons given in post #2 - Even if it were started right at that physical point at rest, the PID is 180 degrees off target, and will push it one way or the other, starting an oscillation.

If the PID output is negated (or the motor wires swapped), the motor will then be driven towards the desired set point, and the PID should (after a bit of tuning) function as desired, either approaching the set point asymptotically or slightly overshooting with a dampening oscillation.

Got it. Thank you!