View Single Post
  #1   Spotlight this post!  
Unread 06-07-2016, 16:45
team-4480's Avatar
team-4480 team-4480 is offline
Debug? What's that?
FRC #4480
 
Join Date: Jan 2015
Rookie Year: 2013
Location: Minnesooota
Posts: 216
team-4480 will become famous soon enoughteam-4480 will become famous soon enough
What is wrong with my Vision to Gyro code?!

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):

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:
Code:
        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
__________________
#Python4Life

Last edited by team-4480 : 06-07-2016 at 17:54.
Reply With Quote