Package com.grt192.sensor

Source Code of com.grt192.sensor.GRTGyro

package com.grt192.sensor;

import java.util.Vector;

import com.grt192.core.Sensor;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.PIDSource;

/**
* GRTGyro is a continuously running sensor driver that collects and serves data
* from a single-axis gyro
*
* @author anand
*/
public class GRTGyro extends Sensor implements PIDSource {

  public static final double SPIKE_THRESHOLD = 1.0;
  public static final double CHANGE_THRESHOLD = .01;
  private Gyro gyro;
  private Vector gyroListeners;

  public static double DRIFT_THRESHOLD = 0.5;

  public GRTGyro(int channel, int pollTime, String id) {
    gyro = new Gyro(channel);
    setSleepTime(pollTime);
    setState("Angle", 0.0);
    gyroListeners = new Vector();
    this.id = id;
  }

  public void poll() {
    double previousValue = getState("Angle");
    double angle = gyro.getAngle() * 360.0 / 208.0;

    setState("Angle", angle);

    if (Math.abs(getState("Angle") - previousValue) >= SPIKE_THRESHOLD) {
      notifyGyroSpike();
    }
    if (Math.abs(getState("Angle") - previousValue) >= CHANGE_THRESHOLD) {
      notifyGyroChange();
    }
    notifyGyroListeners();
  }

  protected void notifyGyroSpike() {
    for (int i = 0; i < gyroListeners.size(); i++) {
      ((GyroListener) gyroListeners.elementAt(i))
          .didAngleSpike(new GyroEvent(this, GyroEvent.SPIKE,
              getState("Angle")));
    }
  }

  protected void notifyGyroChange() {
    for (int i = 0; i < gyroListeners.size(); i++) {
      ((GyroListener) gyroListeners.elementAt(i))
          .didAngleChange(new GyroEvent(this, GyroEvent.CHANGE,
              getState("Angle")));
    }
  }

  protected void notifyGyroListeners() {
    for (int i = 0; i < gyroListeners.size(); i++) {
      ((GyroListener) gyroListeners.elementAt(i))
          .didReceiveAngle(new GyroEvent(this, GyroEvent.NEW_DATA,
              getState("Angle")));
    }
  }

  public Vector getGyroListeners() {
    return gyroListeners;
  }

  public void addGyroListener(GyroListener a) {
    gyroListeners.addElement(a);
  }

  public void removeGyroListener(GyroListener a) {
    gyroListeners.removeElement(a);
  }

  public String toString() {
    return "gyro: " + getState("Angle");
  }

  public double pidGet() {
    // fix asap
    return 0;
  }

}
TOP

Related Classes of com.grt192.sensor.GRTGyro

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.