|
|
|
![]() |
|
|||||||
|
||||||||
|
|
Thread Tools | Rate Thread | Display Modes |
|
#1
|
||||
|
||||
|
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
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
Last edited by team-4480 : 06-07-2016 at 17:54. |
| Thread Tools | |
| Display Modes | Rate This Thread |
|
|