Java 类edu.wpi.first.wpilibj.CounterBase 实例源码

/**
 * Creates an encoder using the two SIG inputs (A and B).
 * Also allows the encoder to be reversed.
 * Can also control the difference between getRaw and get values.
 * pulsesPerRev is used for getDistance() methods.
 * @param channelA I/0 SIG A.
 * @param channelB I/O SIG B.
 * @param isReversed When true, returned values are inverted.
 * @param scaleValue getRaw() values are divided by multiples of 1, 2, or 4 to increase accuracy.
 * @param pulsesPerRev Number of pulses for a revolution of the motor (look at instance variable).
 */
public QuadratureEncoder(int channelA, int channelB, boolean isReversed,
                        int scaleValue, double pulsesPerRev)
{
    CounterBase.EncodingType encType = CounterBase.EncodingType.k4X;

    if (scaleValue == 1)
        encType = CounterBase.EncodingType.k1X;
    else if (scaleValue == 2)
        encType = CounterBase.EncodingType.k2X;
    else if (scaleValue == 4)
        encType = CounterBase.EncodingType.k4X;

    enc = new Encoder(channelA, channelB, isReversed, encType);

    pulsesPerRevolution = pulsesPerRev;
    enc.start();
}

private Drive()
{
    drive = new RobotDrive(new Talon(1),new Talon(2),new Talon(3),new Talon(4));
    drive.setSafetyEnabled(false);


    e1 = new Encoder(RobotMap.ENCODER_1_A, RobotMap.ENCODER_1_B, false, CounterBase.EncodingType.k4X);
    e2 = new Encoder(RobotMap.ENCODER_2_A, RobotMap.ENCODER_2_B, false, CounterBase.EncodingType.k4X);

    //A calculated constant to convert from encoder ticks to feet on 4 inch diameter wheels
    e1.setDistancePerPulse(0.0349065850388889/12);
    e2.setDistancePerPulse(0.0349065850388889/12);
    startEncoders();

    drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);      
    drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);

    sonic = new Ultrasonic(RobotMap.ULTRASONIC_A, RobotMap.ULTRASONIC_B);
    sonic.setAutomaticMode(true);
    sonic.setEnabled(true);


    shifter = new DoubleSolenoid(RobotMap.SHIFTER_ENGAGE, RobotMap.SHIFTER_DISENGAGE);
}

public DriveTrain(boolean isCan)
{

    shifter = new Piston(SHIFTER_EXTEND_PORT, SHIFTER_RETRACT_PORT);
    left = new BTMotor(LEFT_JAG_PORT, isCan, isVoltage);
    left_2 = new BTMotor(LEFT_JAG_PORT_2, isCan, isVoltage);
    right = new BTMotor(RIGHT_JAG_PORT, isCan, isVoltage);
    right_2 = new BTMotor(RIGHT_JAG_PORT_2, isCan, isVoltage);
    shiftSpeed = new Encoder(DRIVE_ENCODER_A_PORT, DRIVE_ENCODER_B_PORT, true, CounterBase.EncodingType.k1X);
    shiftSpeed.setDistancePerPulse(distance);
    shiftSpeed.start();
    shiftSpeed.reset();

    left.setBTVoltageRampRate(ramprate);
    left_2.setBTVoltageRampRate(ramprate);
    right.setBTVoltageRampRate(ramprate);
    right_2.setBTVoltageRampRate(ramprate);
}

public EncoderWrapper(int aIndexA, int aIndexB)
{
    super("Encoder (" + aIndexA + ", " + aIndexB + ")");

    setEncodingFactor(CounterBase.EncodingType.k4X);
    mDistancePerTick = 1;
}

public EncoderPIDSubsystem(String name, double kP, double kI, double kD, int motorChannel, int encoderAChannel, int encoderBChannel, boolean reverseEncoder, double gearRatioEncoderToOutput) {
    super(name, kP, kI, kD);
    try {
        m_motorController = new Victor(motorChannel);
        m_encoder = new Encoder(1, encoderAChannel, 1, encoderBChannel, reverseEncoder, CounterBase.EncodingType.k4X);
        double degPerPulse = 360.0 / gearRatioEncoderToOutput / DEFAULT_PULSES_PER_ROTATION;
        m_encoder.setDistancePerPulse(degPerPulse);
        resetZeroPosition();
    } catch (Exception e) {
        System.out.println("Unknown error initializing " + getName() + "-EncoderPIDSubsystem.  Message = " + e.getMessage());
    }
}

public EncoderPIDSubsystem(String name, double kP, double kI, double kD, int motorChannel, int encoderAChannel, int encoderBChannel, boolean reverseEncoder, double wheelDiameter, double gearRatioEncoderToWheel) {
    super(name, kP, kI, kD);
    try {
        m_motorController = new Victor(motorChannel);
        m_encoder = new Encoder(1, encoderAChannel, 1, encoderBChannel, reverseEncoder, CounterBase.EncodingType.k4X);
        double distancePerPulse = Math.PI * wheelDiameter / gearRatioEncoderToWheel / DEFAULT_PULSES_PER_ROTATION;
        m_encoder.setDistancePerPulse(distancePerPulse);
        resetZeroPosition();
    } catch (Exception e) {
        System.out.println("Unknown error initializing " + getName() + "-EncoderPIDSubsystem.  Message = " + e.getMessage());
    }
}

public RPMEncoder(int aSlot, int aChannel, int bSlot, int bChannel, boolean reverseDirection, double pulsesPerRotation, int numAveragedCycles) {
        m_pulsesPerRotation = pulsesPerRotation;
        m_numAveragedCycles = numAveragedCycles;

        m_encoder = new Encoder(aSlot, aChannel, bSlot, bChannel, reverseDirection, CounterBase.EncodingType.k4X);

        m_motorRPM = new double[numAveragedCycles];
        m_encoder.reset();
        resetMotorRPM();
}

public O_TurnEncoder(int APort, int BPort, int zeroPort, double zeroOffset, boolean reverseEncoder){       
    zeroSensor = new DigitalInput(RobotMap.turnModule, zeroPort);
    this.zeroOffset = zeroOffset;
    encoder = new Encoder(RobotMap.turnModule, APort, RobotMap.turnModule, BPort, reverseEncoder, CounterBase.EncodingType.k4X) {{
        setDistancePerPulse(360.0 / 500.0);
        start();
    }};
}

public RobotTemplate() {
    // initialize all the objects
    ingest = new VictorPair(5,6);
    elevator = new Victor(1);

    shooter = new VictorPair(2,4);

    robotDrive = new Drive(8, 7, 10, 9);
    xbox = new JStick(1);
    joystick = new JStick(2);
    compressor = new Compressor(4, 3);
    compressor.start();

    driveGearA = new Solenoid(1);
    driveGearB = new Solenoid(2);
    driveGearA.set(true);
    driveGearB.set(false);
    selectedGear = 1;

    leftEnc = new Encoder(6, 7, true,CounterBase.EncodingType.k2X);
    leftEnc.setDistancePerPulse(0.103672558);

    rightEnc = new Encoder(9, 10, false);
    rightEnc.setDistancePerPulse(0.103672558);

    lcd = DriverStationLCD.getInstance();
}

/**
 * Instantiates an encoder on the default digital module.
 *
 * @param channelA digital channel for encoder channel A
 * @param channelB digital channel for encoder channel B
 * @param pulseDistance distance traveled for each pulse (typically 1 degree
 * of rotation per pulse)
 * @param reversed whether or not the encoder is reversed
 * @param name name of encoder
 */
public GRTEncoder(int channelA, int channelB,
        double pulseDistance, boolean reversed, String name) {
    super(name, NUM_DATA);

    //Create new encoder that does 1x encoding, as opposed to 4x.
    rotaryEncoder = new Encoder(channelA, channelB, reversed,
            CounterBase.EncodingType.k1X);  
    rotaryEncoder.start();

    encoderListeners = new Vector();
    distancePerPulse = pulseDistance;
    rotaryEncoder.setDistancePerPulse(distancePerPulse);
}

/**
 * Instantiates an encoder on the default digital module.
 *
 * @param channelA digital channel for encoder channel A
 * @param channelB digital channel for encoder channel B
 * @param pulseDistance distance traveled for each pulse (typically 1 degree
 * of rotation per pulse)
 * @param reversed whether or not the encoder is reversed
 * @param name name of encoder
 */
public GRTEncoder(int channelA, int channelB,
        double pulseDistance, boolean reversed, String name) {
    super(name, NUM_DATA);

    //Create new encoder that does 1x encoding, as opposed to 4x.
    rotaryEncoder = new Encoder(channelA, channelB, reversed,
            CounterBase.EncodingType.k1X);  
    rotaryEncoder.start();

    encoderListeners = new Vector();
    distancePerPulse = pulseDistance;
    rotaryEncoder.setDistancePerPulse(distancePerPulse);
}

public TestBotSwervePod(int talonPWM, SabertoothSpeedController.Address sabertoothAddress, SabertoothSpeedController.Motor sabertoothMotor, int encoderPin1, int encoderPin2, int digipotPin, double digipotOffset) {
    super(new Talon(talonPWM), new SabertoothSpeedController(sabertoothAddress, sabertoothMotor), new Encoder(encoderPin1, encoderPin2, false, CounterBase.EncodingType.k1X), ENCODER_INCHES_PER_TICK, new BI6120Magnepot(digipotPin, digipotOffset));
}

public PositionEncoder(int aSlot, int aChannel, int bSlot, int bChannel, boolean reverseDirection, double pulsesPerRotation, double wheelDiameter, double gearRatioWheelToEncoder) {
        m_encoder = new Encoder(aSlot, aChannel, bSlot, bChannel, reverseDirection, CounterBase.EncodingType.k4X);
        double distancePerPulse = Math.PI * wheelDiameter * wheelDiameter * gearRatioWheelToEncoder / 4.0 / pulsesPerRotation;
        m_encoder.setDistancePerPulse(distancePerPulse);
        resetDistance();
}

public Chassis() {
        super(KP, KI, KD);

        try {
            m_drive = new RobotDrive(
                    new Victor(RobotMap.DRIVE_LEFT_TOP_FRONT_DSC_PWM_ID),
                    new Victor(RobotMap.DRIVE_LEFT_REAR_DSC_PWM_ID),
                    new Victor(RobotMap.DRIVE_RIGHT_TOP_FRONT_DSC_PWM_ID),
                    new Victor(RobotMap.DRIVE_RIGHT_REAR_DSC_PWM_ID));

            m_drive.setSafetyEnabled(false);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

            m_leftEncoder = new Encoder(
                    1, RobotMap.LEFT_DRIVE_ENCODER_A_DSC_DIO_ID, 
                    1, RobotMap.LEFT_DRIVE_ENCODER_B_DSC_DIO_ID, true, CounterBase.EncodingType.k4X);
            m_rightEncoder = new Encoder(
                    1, RobotMap.RIGHT_DRIVE_ENCODER_A_DSC_DIO_ID, 
                    1, RobotMap.RIGHT_DRIVE_ENCODER_B_DSC_DIO_ID, false, CounterBase.EncodingType.k4X);    

            m_leftEncoder.setDistancePerPulse(WHEEL_CIRCUM_IN / 360.0 / ENCODER_TO_WHEEL_GEAR_RATIO);
            m_rightEncoder.setDistancePerPulse(WHEEL_CIRCUM_IN / 360.0 / ENCODER_TO_WHEEL_GEAR_RATIO);

            resetEncoders();

            m_yawGyro = new Gyro(RobotMap.CHASSIS_YAW_RATE_ANALOG_BREAKOUT_PORT);
            m_yawGyro.setSensitivity(0.007);  // 7 mV/deg/sec

//            SmartDashboard.putNumber("Move NonLinear ", m_moveNonLinear);
//            SmartDashboard.putNumber("Steer NonLinear ", m_steerNonLinear);
//            SmartDashboard.putNumber("Move Scale ", m_moveScale);
//            SmartDashboard.putNumber("Steer Scale ", m_steerScale);
//            SmartDashboard.putNumber("Move Trim ", -m_moveTrim);
//            SmartDashboard.putNumber("Steer Trim ", m_steerTrim); 
        } catch (Exception e) {
            System.out.println("Unknown error initializing chassis.  Message = " + e.getMessage());
        }
    }

private void init() {
        if (!initialized) {
            flTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.FL_WHEEL);
            frTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.FR_WHEEL);
            blTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.BL_WHEEL);
            brTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.BR_WHEEL);

            System.out.println("Module: "+ RobotMap.DIGITAL_MODULE +" used for drive.");
            System.out.println("FL wheel at: "+ RobotMap.FL_WHEEL +" FR wheel at: "+ RobotMap.FR_WHEEL);
            System.out.println("BL wheel at: "+ RobotMap.BL_WHEEL +" BR wheel at: "+ RobotMap.BR_WHEEL);

//            if (useRobotDrive)
                robotDrive = new RobotDrive(flTalon, blTalon, frTalon, brTalon);
//            else
//                robotDrive = null;

            aButton_Held = false;
            driveForward = true;
            SmartDashboard.putBoolean("Drive_Direction", driveForward);

            leftEncoder = new Encoder(RobotMap.DIGITAL_MODULE, RobotMap.LEFT_ENCODER_A,
                    RobotMap.DIGITAL_MODULE, RobotMap.LEFT_ENCODER_B, true, CounterBase.EncodingType.k4X);
            rightEncoder = new Encoder(RobotMap.DIGITAL_MODULE, RobotMap.RIGHT_ENCODER_A,
                    RobotMap.DIGITAL_MODULE, RobotMap.RIGHT_ENCODER_B, false, CounterBase.EncodingType.k4X);

    //        leftEncoder.setReverseDirection(false);
            leftEncoder.setReverseDirection(true); // Flipped on comp bot
            leftEncoder.setMinRate(10);
            leftEncoder.setDistancePerPulse(6 * Math.PI / 250);
            leftEncoder.start();
    //        rightEncoder.setReverseDirection(true);
            rightEncoder.setReverseDirection(false); // Flipped on comp bot
            rightEncoder.setMinRate(10);
            rightEncoder.setDistancePerPulse(6 * Math.PI / 250);
            rightEncoder.start();

            gyro = new Gyro(RobotMap.ANALOG_MODULE, RobotMap.GYRO);

            SmartDashboard.putNumber("Max turn speed", 1);
        }
        initialized = true;
    }

public static void init() {

    //initialize encoders
    frontWheelEncoder = new Encoder(1, 1, 1, 2, false, CounterBase.EncodingType.k2X);
    frontWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK);
    frontWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    frontWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "frontWheelEncoder", frontWheelEncoder);
    leftWheelEncoder = new Encoder(1, 3, 1, 4, false, CounterBase.EncodingType.k2X);
    leftWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK); 
    leftWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    leftWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "leftWheelEncoder", leftWheelEncoder);
    backWheelEncoder = new Encoder(1, 5, 1, 6, false, CounterBase.EncodingType.k2X);
    backWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK); 
    backWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    backWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "backWheelEncoder", backWheelEncoder); 
    rightWheelEncoder = new Encoder(1, 7, 1, 8, false, CounterBase.EncodingType.k2X);
    rightWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK); 
    rightWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    rightWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "rightWheelEncoder", rightWheelEncoder); 

    frontModuleEncoder = new Encoder(2, 1, 2, 2, false, CounterBase.EncodingType.k2X);
    frontModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    frontModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    frontModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "frontModuleEncoder", frontModuleEncoder);
    leftModuleEncoder = new Encoder(2, 7, 2, 8, false, CounterBase.EncodingType.k2X);
    leftModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    leftModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    leftModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "leftModuleEncoder", leftModuleEncoder);
    backModuleEncoder = new Encoder(2, 5, 2, 6, false, CounterBase.EncodingType.k2X);
    backModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    backModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    backModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "backModuleEncoder", backModuleEncoder);
    rightModuleEncoder = new Encoder(2, 3, 2, 4, false, CounterBase.EncodingType.k2X);
    rightModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    rightModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    rightModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "rightModuleEncoder", rightModuleEncoder);

    //initialize motors
    frontWheelMotor = new Victor246(1,1);
    LiveWindow.addActuator("Drivetrain", "frontWheelMotor", (Victor) frontWheelMotor);
    leftWheelMotor = new Victor246(1,2);
    LiveWindow.addActuator("Drivetrain", "leftWheelMotor", (Victor) leftWheelMotor);
    backWheelMotor = new Victor246(1,3);
    LiveWindow.addActuator("Drivetrain", "backWheelMotor", (Victor) backWheelMotor);
    rightWheelMotor = new Victor246(1,4);
    LiveWindow.addActuator("Drivetrain", "rightWheelMotor", (Victor) rightWheelMotor);

    frontModuleMotor = new Jaguar246(2,1);
    LiveWindow.addActuator("Drivetrain", "frontModuleMotor", (Jaguar) frontModuleMotor);
    leftModuleMotor = new Jaguar246(2,2);
    LiveWindow.addActuator("Drivetrain", "leftModuleMotor", (Jaguar) leftModuleMotor);
    backModuleMotor = new Jaguar246(2,3);
    LiveWindow.addActuator("Drivetrain", "backModuleMotor", (Jaguar) backModuleMotor);
    rightModuleMotor = new Jaguar246(2,4);
    LiveWindow.addActuator("Drivetrain", "rightModuleMotor", (Jaguar) rightModuleMotor);

    //initialize limit switch
    angleZeroingButton = new DigitalInput(1,9);
    LiveWindow.addSensor("Drivetrain", "encoderZeroingSwitch", angleZeroingButton);
}

public Builder leftEncoder(int a, int b) {
    drive.leftEncoder = new Encoder(a, b, true, CounterBase.EncodingType.k1X);
    return this;
}

public Builder rightEncoder(int a, int b) {
    drive.rightEncoder = new Encoder(a, b, false, CounterBase.EncodingType.k1X);
    return this;
}

public Builder leftEncoder(int a, int b) {
    drive.leftEncoder = new Encoder(a, b, true, CounterBase.EncodingType.k1X);
    return this;
}

public Builder rightEncoder(int a, int b) {
    drive.rightEncoder = new Encoder(a, b, false, CounterBase.EncodingType.k1X);
    return this;
}