drive.py

import wpilib
import ctre
from robotpy_ext.common_drivers.navx import *

class Drive:
    '''
    classdocs
    '''
    navx = AHRS
    def setup(self):
        self.timer = wpilib.Timer()
        
    def get_position(self):
        return (self.navx.getDisplacementX(), self.navx.getDisplacementY(), self.navx.getDisplacementZ())
        
    def _verlet(self, r, v, dt, a):
        """Return new position and velocity from current values, time step and acceleration.
        Parameters:
           r is a numpy array giving the current position vector
           v is a numpy array giving the current velocity vector
           dt is a float value giving the length of the integration time step
           a is a function which takes x as a parameter and returns the acceleration vector as an array
           
           Works with arrays of any dimension as long as they're all the same.
        """
        # Deceptively simple (read about Velocity Verlet on wikipedia)
        r_new = r + v*dt + a(r)*dt**2/2
        v_new = v + (a(r) + a(r_new))/2 * dt
        return (r_new, v_new)
    
    def execute(self):
        pass