IMU.java

package driver;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.DigitalInput;

/**
 *
 * @author matthew
 */
public class IMU {

    private final Counter pH;
    private final Counter pL;
    private static float offset = 0;

    public IMU(int channel) {
        // Setup source input in order to prevent blocking later
        DigitalInput source = new DigitalInput(channel);
        
        // Two counters for high and low of pwm signal
        pH = new Counter();
        pL = new Counter();
        
        // bind input source to two counters
        pH.setUpSource(source);
        pL.setUpSource(source);

        /* set one to detect low to high pin 
           changes and one to detect high to
           low pin changes
        */
        pH.setSemiPeriodMode(true);
        pL.setSemiPeriodMode(false);
    }

    // Start counters
    public void start() {
        pH.start();
        pL.start();
    }

    // Stop counters
    public void end() {
        pH.stop();
        pL.stop();
    }

    // Zero angle
    public final void zero() {
        offset = getAngle();
    }

    // Get pulse width in seconds
    public float getWidth() {
        return (float) (pH.getPeriod() / (pH.getPeriod() + pL.getPeriod()));
    }

    // Get angle in degrees
    public float getAngle() {
        return 360 * getWidth() + offset;
    }
}