Java 类edu.wpi.first.wpilibj.interfaces.Accelerometer 实例源码

public void update(){

    // Shift all of the values to the left once
    for (int i = 0; i < numberOfSamples - 1; i++) {
        xValues[i] = xValues[i + 1];
        yValues[i] = yValues[i + 1];
        zValues[i] = zValues[i + 1];
    }

    // Update all of the values with accelerometer
    xValues[numberOfSamples - 1] = this.getX();
    yValues[numberOfSamples - 1] = this.getY();
    zValues[numberOfSamples - 1] = this.getZ();

    // If all of the latest values are 0, then the accelerometer crashed. DisablePID and stop the  shooter aim motor, then try to re-initialize it.
    if (xValues[numberOfSamples - 1] == 0 && yValues[numberOfSamples - 1] == 0 && zValues[numberOfSamples - 1] == 0) {

        Robot.shooterAim.disablePID();
        Robot.shooterAim.manualAim(0);

        accelerometer = new ADXL345_I2C(I2C.Port.kOnboard, Accelerometer.Range.k4G);

    }

}

/**
 * Instantiates the Sensor Input module to read all sensors connected to the roboRIO.
 */
private SensorInput() { 
    limit_left = new DigitalInput(ChiliConstants.left_limit);
    limit_right = new DigitalInput(ChiliConstants.right_limit);
    accel = new BuiltInAccelerometer(Accelerometer.Range.k2G);
    gyro = new Gyro(ChiliConstants.gyro_channel);
    pdp = new PowerDistributionPanel();
    left_encoder = new Encoder(ChiliConstants.left_encoder_channelA, ChiliConstants.left_encoder_channelB, false);
    right_encoder = new Encoder(ChiliConstants.right_encoder_channelA, ChiliConstants.right_encoder_channelB, true);
    gyro_i2c = new GyroITG3200(I2C.Port.kOnboard);

    gyro_i2c.initialize();
    gyro_i2c.reset();
    gyro.initGyro();

    left_encoder.setDistancePerPulse(ChiliConstants.kDistancePerPulse);
    right_encoder.setDistancePerPulse(ChiliConstants.kDistancePerPulse);
}

/**
 * Creates a new Positioner object.
 * 
 * @param aGyro
 *            The Gyro to use.
 * @param aDriveTrain
 *            The DriveTrain to use.
 * @param aLogger
 *            The robot's Logger.
 * @param aAccelerometer
 *            The robot's accelerometer
 */
public Positioner(Gyro aGyro, IDriveTrain aDriveTrain, Logger aLogger, Accelerometer aAccelerometer)
{
    mXPosition = 0;
    mYPosition = 0;
    mOrientation = 0;
    mTotalDistance = 0;
    mLastDistance = 0;
    mLastTime = 0;
    mSpeed = 0;
    mGyro = aGyro;
    mDriveTrain = aDriveTrain;
    mTimer = new Timer();
    mLogger = aLogger;
    mStartAngle = 0;

    mVelocityX = 0;
    mVelocityY = 0;

    mAccelerometer = aAccelerometer;
    mErrorX = 0;
    mErrorY = 0;
    mErrorZ = 0;

    // TODO Get rid of these when done testing
    encoderXVelocity = 0;
    encoderYVelocity = 0;
    accelXVelocity = 0;
    accelYVelocity = 0;
    avgFiltXVelocity = 0;
    avgFiltYVelocity = 0;
    compFiltXVelocity = 0;
    compFiltYVelocity = 0;
    mAvgFiltXPos = 0;
    mAvgFiltYPos = 0;
    mCompFiltXPos = 0;
    mCompFiltYPos = 0;

    isFirstTime = true;
}

public static void init() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    driveTrainFrontLeftMotor = new Jaguar(2);
    LiveWindow.addActuator("DriveTrain", "FrontLeftMotor", (Jaguar) driveTrainFrontLeftMotor);

    driveTrainRearLeftMotor = new Jaguar(3);
    LiveWindow.addActuator("DriveTrain", "RearLeftMotor", (Jaguar) driveTrainRearLeftMotor);

    driveTrainFrontRightMotor = new Jaguar(1);
    LiveWindow.addActuator("DriveTrain", "FrontRightMotor", (Jaguar) driveTrainFrontRightMotor);

    driveTrainRearRightMotor = new Jaguar(0);
    LiveWindow.addActuator("DriveTrain", "RearRightMotor", (Jaguar) driveTrainRearRightMotor);

    /*driveTrainRearLeftPID = new PIDSpeedController(backLeftEncoder, driveTrainRearLeftMotor, "Drive", "Back Left");
    LiveWindow.addActuator("DriveTrain", "RearLeftPID", (Jaguar) driveTrainRearLeftMotor);

    driveTrainRearRightMotor = new PIDSpeedController(backRightEncoder, driveTrainRearRightMotor, "Drive", "Back Right");
    LiveWindow.addActuator("DriveTrain", "RearRightPID", (Jaguar) driveTrainRearRightMotor);*/

    driveTrainrobotDrive = new RobotDrive(driveTrainFrontLeftMotor, driveTrainRearLeftMotor,
          driveTrainFrontRightMotor, driveTrainRearRightMotor);

    driveTrainrobotDrive.setSafetyEnabled(true);
    driveTrainrobotDrive.setExpiration(0.1);
    driveTrainrobotDrive.setSensitivity(0.3);
    driveTrainrobotDrive.setMaxOutput(1.0);
    driveTrainrobotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
    driveTrainrobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);

    accel = new BuiltInAccelerometer();
    accel = new BuiltInAccelerometer(Accelerometer.Range.k4G);
    LiveWindow.addSensor("DriveTrain", "Accelerometer", (LiveWindowSendable) accel); 

    gyro = new Gyro(0);
    LiveWindow.addSensor("DriveTrain", "Gyro", (LiveWindowSendable) gyro); 

    liftMotor = new Talon(4);
    LiveWindow.addActuator("Lift", "Lift Motor", (Talon) liftMotor);

    topLimit = new DigitalInput(8);
    LiveWindow.addSensor("Lift", "Upper Limit Switch", topLimit);
    bottomLimit = new DigitalInput(9);
    LiveWindow.addSensor("Lift", "Lower Limit Switch", bottomLimit);

    toteInRobot = new DigitalInput(4);
    LiveWindow.addSensor("Lift", "Tote In Robot", toteInRobot);
    lotOfTotes = new DigitalInput(5);
    LiveWindow.addSensor("Lift", "5 Totes in Robot", lotOfTotes);

    liftEncoder = new Encoder(6,7);
    LiveWindow.addSensor("Lift", "Lift Encoder", liftEncoder);
    backLeftEncoder = new Encoder(2,3);
    LiveWindow.addSensor("Lift", "Back Left Encoder", backLeftEncoder);
    backRightEncoder = new Encoder(0,1);
    LiveWindow.addSensor("Lift", "Back Right Encoder", backRightEncoder);

    leftPicker = new Talon(5);
    LiveWindow.addActuator("Pickers", "Left Picker", (Talon) leftPicker);
    rightPicker = new Talon(6);
    LiveWindow.addActuator("Pickers", "Right Picker", (Talon) rightPicker);

    compressor = new Compressor(0);
    LiveWindow.addActuator("Lift", "Compressor", compressor);
    dogs = new Solenoid(0);
    LiveWindow.addActuator("Lift", "Dogs", dogs);
    pusher = new Solenoid(1);
    LiveWindow.addActuator("Lift", "Pusher", pusher);
    flipper = new Solenoid(2);
    LiveWindow.addActuator("Lift", "Flipper", flipper);

    thatArmThatOpensToReleaseStacks = new Solenoid(3);
    LiveWindow.addActuator("Lift", "Stack Arm", thatArmThatOpensToReleaseStacks);

}

/**
 * Creates a new Positioner object.
 * 
 * @param aGyro
 *            The Gyro to use.
 * @param aAccelerometer
 *            The accelerometer to use.
 * @param aDriveTrain
 *            The DriveTrain to use.
 * @param aLogger
 *            The robot's Logger.
 */
public IMUPositioner(Gyro aGyro, Accelerometer aAccelerometer, IDriveTrain aDriveTrain, Logger aLogger)
{
    mGyro = aGyro;
    mAccelerometer = aAccelerometer;
    mDriveTrain = aDriveTrain;
    mLogger = aLogger;
    mTimer = new Timer();
}