Go to Post In my opinion, the right mix of student, mentor, and engineer involvement is when everyone gives it 100%. - Jack Jones [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

 
 
 
Thread Tools Rate Thread Display Modes
Prev Previous Post   Next Post Next
  #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
 


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 09:52.

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