OCCRA
Go to Post Plugging your own design when people are recognizing others, especially when you are such a seasoned member of the forums just doesn't cut it anymore. - J Flex 188 [more]
Home
Go Back   Chief Delphi > Technical > Programming > Python
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 03-22-2018, 09:16 AM
codetheweb's Avatar
codetheweb codetheweb is offline
Registered User
AKA: Max Isom
FRC #3299 (The Warehouse Crew)
Team Role: Programmer
 
Join Date: Mar 2015
Rookie Year: 2014
Location: Chaska, MN
Posts: 14
codetheweb is an unknown quantity at this point
Pythonic way of using PID loops in auto

I realized this would be much easier if our team had used the commandbased framework shipped with RobotPy, but it's too late to change now.

We're trying to execute actions sequentially in auto. The issue is that our two main actions, driving a certain distance and turning to an angle, are both closed-loop controlled with a PID controller instead of being time-based. If it were time based, we'd use the StatefulAutonomous class bundled with RobotPy. But since we're using PID loops, I can't think of a better way to handle this than just running nested forms of
Code:
while self.drive.driveToAngle(60) != True:
.

What is the more Pythonic and RobotPy way of doing this? Would some form of decorators be appropriate?

This is what I ultimately would like our auto code to look like.
Code:
self.drive.driveToAngle(60)
# executed after robot has finished turning
self.drive.driveToPosition(5)
Relevant code for driving:
Code:
class Chassis(object):
    def __init__(self, drive, gyro, encoderY):
        self.drive          = drive
        self.gyro           = gyro
        self.encoderY       = encoderY


        # PID loop for angle
        self.pidAngleDefault = {'p': 0.03, 'i': 0, 'd': 0.1}
        self.sd.putNumber('pidAngleP', self.pidAngleDefault['p'])
        self.sd.putNumber('pidAngleI', self.pidAngleDefault['i'])
        self.sd.putNumber('pidAngleD', self.pidAngleDefault['d'])

        self.pidAngle = wpilib.PIDController(self.pidAngleDefault['p'], self.pidAngleDefault['i'], self.pidAngleDefault['d'], self.gyro, self.updateAnglePID)
        self.pidAngle.setContinuous(False)
        self.pidRotateRate = 0

        # PID loop for Cartesian Y direction
        self.pidYDefault = {'p': 0.05, 'i': 0, 'd': 0.02}
        self.sd.putNumber('pidYP', self.pidYDefault['p'])
        self.sd.putNumber('pidYI', self.pidYDefault['i'])
        self.sd.putNumber('pidYD', self.pidYDefault['d'])

        self.pidY = wpilib.PIDController(self.pidYDefault['p'], self.pidYDefault['i'], self.pidYDefault['d'], self.encoderY.getRate, self.updateYPID)
        self.pidY.setAbsoluteTolerance(0.3)
        self.pidY.setContinuous(False)
        self.pidYRate = 0

    def driveToAngle(self, angle):
        """Intended for use in auto."""
        self.gyro.reset()
        self.pidAngle.setSetpoint(angle)
        self.pidAngle.enable()

        while True:
            if (self.pidAngle.onTarget()):
                self.pidAngle.disable()
                return True
            else:
                self.cartesian(0, 0, -self.pidRotateRate)

    def driveToPosition(self, distance):
        """Intended for use in auto."""
        self.pidY.setSetpoint(distance)
        while True:
            if (self.pidY.onTarget()):
                self.pidY.disable()
                return True

    def updateAnglePID(self, value):
        self.pidAngle.setP(self.sd.getNumber('pidAngleP', self.pidAngleDefault['p']))
        self.pidAngle.setI(self.sd.getNumber('pidAngleI', self.pidAngleDefault['i']))
        self.pidAngle.setD(self.sd.getNumber('pidAngleD', self.pidAngleDefault['d']))

        self.pidRotateRate = value

    def updateYPID(self, value):
        self.pidY.setP(self.sd.getNumber('pidP', self.pidYDefault['p']))
        self.pidY.setI(self.sd.getNumber('pidI', self.pidYDefault['i']))
        self.pidY.setD(self.sd.getNumber('pidD', self.pidYDefault['d']))

        self.pidYRate = value
Reply With Quote
  #2   Spotlight this post!  
Unread 03-22-2018, 03:48 PM
nickbrickmaster nickbrickmaster is offline
Registered User
AKA: Nick Schatz
no team ('Snow Problem, 3184 Alum)
Team Role: Programmer
 
Join Date: Jan 2015
Rookie Year: 2014
Location: Eagan MN
Posts: 464
nickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond reputenickbrickmaster has a reputation beyond repute
Re: Pythonic way of using PID loops in auto

The best way to do something like this is build yourself a really simple sequential state machine.

(Inside the iterative loop)
Let state = 0 at the beginning.
If state = 0, execute your first action. If you're done with this action, let your state += 1.
If state = 1, execute your second action. If you're done with this action, let your state += 1.
etc

In general, using while loops is bad practice and will actually hang your code.
__________________
This is a postmodern signature.
Reply With Quote
  #3   Spotlight this post!  
Unread 03-22-2018, 06:36 PM
codetheweb's Avatar
codetheweb codetheweb is offline
Registered User
AKA: Max Isom
FRC #3299 (The Warehouse Crew)
Team Role: Programmer
 
Join Date: Mar 2015
Rookie Year: 2014
Location: Chaska, MN
Posts: 14
codetheweb is an unknown quantity at this point
Re: Pythonic way of using PID loops in auto

Oh, so we should use autonomousPeriodic instead of autonomousInit.

I knew that
Quote:
Originally Posted by nickbrickmaster View Post
In general, using while loops is bad practice and will actually hang your code.
but had not connected the dots for some reason. Thank you!
Reply With Quote
  #4   Spotlight this post!  
Unread 03-22-2018, 11:05 PM
virtuald's Avatar
virtuald virtuald is offline
RobotPy Guy
AKA: Dustin Spicuzza
FRC #6367 (), FRC #7240 ()
Team Role: Mentor
 
Join Date: Dec 2008
Rookie Year: 2003
Location: Boston, MA
Posts: 1,395
virtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond reputevirtuald has a reputation beyond repute
Re: Pythonic way of using PID loops in auto

Stateful autonomous provides a really easy way to create a bunch of states. The states don't have to be time based, you can just call 'next_state' at any time to move between states.
__________________
Maintainer of RobotPy (Python for FRC) & WPILib Contributor
Creator of pyfrc (Robot Simulator + utilities for Python), pynetworktables/pynetworktables2js (NetworkTables for Python & Javascript), and lots more...

Team #1418 (remote mentor): Newton Quarterfinalists, 2016 Chesapeake District Champion, 2x Innovation in Control award, 2x district event winner
Team #1418: 2015 DC Regional Innovation In Control Award, #2 seed; 2014 VA Industrial Design Award; 2014 Finalists in DC & VA
Team #2423: 2012 & 2013 Boston Regional Innovation in Control Award


Resources: FIRSTWiki (relaunched!) | My Software Stuff
Reply With Quote
Reply


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 02:33 PM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2018, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi