Checking timing of IterativeRobot

robot.py

#!/usr/bin/env python3

import collections
import time
import wpilib

class MyRobot(wpilib.IterativeRobot):
    '''Main robot class'''
    
    def robotInit(self):
        '''Robot-wide initialization code should go here'''
        
        self.lstick = wpilib.Joystick(1)
        self.motor = wpilib.Jaguar(3)

    def autonomousInit(self):
        '''Called only at the beginning of autonomous mode'''
        pass

    def autonomousPeriodic(self):
        '''Called every 20ms in autonomous mode'''
        pass

    def disabledInit(self):
        '''Called only at the beginning of disabled mode'''
        pass
    
    def disabledPeriodic(self):
        '''Called every 20ms in disabled mode'''
        pass

    def teleopInit(self):
        '''Called only at the beginning of teleoperated mode'''
        self.timer = wpilib.Timer()
        self.timer.start()
        
        self.avg = collections.deque(maxlen=20)
        # not strictly true
        self.last = wpilib.Timer.getFPGATimestamp()

    def teleopPeriodic(self):
        '''Called every 20ms in teleoperated mode'''
        
        
        now = wpilib.Timer.getFPGATimestamp()
        
        self.avg.append(now - self.last)
        self.last = now
        
        if self.timer.hasPeriodPassed(1.0):
            tmin = min(self.avg)
            tmax = max(self.avg)
            avg = sum(self.avg)/len(self.avg)
            
            print("Min/Max/Avg: %0.3f %0.3f %0.3f" % (tmin, tmax, avg))
        
        # Move a motor with a Joystick
        self.motor.set(self.lstick.getY())
        
        

if __name__ == '__main__':
    wpilib.run(MyRobot)