Go to Post Stop being LAWYERS and start being ENGINEERS! - dlavery [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
Human left pad
AKA: Nick Schatz
no team (3184 Alum)
Team Role: Programmer
 
Join Date: Jan 2015
Rookie Year: 2014
Location: Eagan MN
Posts: 424
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.
__________________
Proceed as if success is inevitable.
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 online now
RobotPy Guy
AKA: Dustin Spicuzza
FRC #6367 (), FRC #7240 ()
Team Role: Mentor
 
Join Date: Dec 2008
Rookie Year: 2003
Location: Boston, MA
Posts: 1,382
virtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant future
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 07:51 AM.

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