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

public Shooter() {

        shooterLeftSide = new Talon(RobotMap.Pwm.LeftShooterMotor);
        shooterRightSide = new Talon(RobotMap.Pwm.RightShooterMotor);

        encoderLeft = new Encoder(RobotMap.Digital.ShooterLeftChannelA, RobotMap.Digital.ShooterLeftChannelB);
        encoderRight = new Encoder(RobotMap.Digital.ShooterRightChannelA, RobotMap.Digital.ShooterRightChannelB);

        encoderLeft.setPIDSourceType(PIDSourceType.kRate);
        encoderRight.setPIDSourceType(PIDSourceType.kRate);
        encoderLeft.setDistancePerPulse(distancePerPulse);
        encoderRight.setDistancePerPulse(distancePerPulse);

        // leftPID = new PIDSpeedController(encoderLeft, shooterLeftSide, "Shooter", "Left Wheel");
        // rightPID = new PIDSpeedController(encoderRight, shooterRightSide, "Shooter", "Right Wheel");

    }

public Drivetrain() {   
    leftWheelsPIDSource = new EncoderPIDSource(RobotMap.driveEncoderLeft, 1.0 / 14.0, PIDSourceType.kDisplacement);
    //rightWheelsPIDSource = new EncoderPIDSource(RobotMap.driveEncoderRight, -1.0 / 14.0, PIDSourceType.kDisplacement);
    rightWheelsPIDSource = new EncoderPIDSource(RobotMap.driveEncoderRight, ((-1.0 / 360.0) * 250.0) * (1.0 / 14.0), PIDSourceType.kDisplacement);

    compassPID = new PIDController(0.1, 0, 0, new CompassPIDSource(), new DummyPIDOutput());
    gyroPID = new PIDController(0.01, 0.0001, 0.00001, new GyroPIDSource(), new DummyPIDOutput());
    leftWheelsPID = new PIDController(0.02, 0.0004, 0, leftWheelsPIDSource, new DummyPIDOutput());
    rightWheelsPID = new PIDController(0.02, 0.0004, 0, rightWheelsPIDSource, new DummyPIDOutput());

    compassPID.disable();
    compassPID.setOutputRange(-1.0, 1.0); // Set turning speed range
    compassPID.setPercentTolerance(5.0); // Set tolerance of 5%

    gyroPID.disable();
    gyroPID.setOutputRange(-1.0, 1.0); // Set turning speed range
    gyroPID.setPercentTolerance(5.0); // Set tolerance of 5%

    leftWheelsPID.disable();
    leftWheelsPID.setOutputRange(-1.0, 1.0);

    rightWheelsPID.disable();
    rightWheelsPID.setOutputRange(-1.0, 1.0);
}

/**
 * Initialize the gyro. Calibration is handled by calibrate().
 */
public void initGyro() {
  result = new AccumulatorResult();

  m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
  m_analog.setAverageBits(kAverageBits);
  m_analog.setOversampleBits(kOversampleBits);
  double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
  AnalogInput.setGlobalSampleRate(sampleRate);
  Timer.delay(0.1);

  setDeadband(0.0);

  setPIDSourceType(PIDSourceType.kDisplacement);

  UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
  LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
}

public Lift(int motorChannel, int brakeChannelForward, int brakeChannelReverse, int encoderChannelA,
        int encoderChannelB, int boundaryLimitChannel, String subsystem) {
    motor = new Talon(motorChannel);
    brake = new DoubleSolenoid(RobotMap.solenoid.Pcm24, brakeChannelForward, brakeChannelReverse);
    encoder = new Encoder(encoderChannelA, encoderChannelB);
    boundaryLimit = new DigitalInput(boundaryLimitChannel);

    LiveWindow.addActuator(subsystem, "Motor", motor);
    LiveWindow.addActuator(subsystem, "Brake", brake);
    LiveWindow.addSensor(subsystem, "Encoder", encoder);
    LiveWindow.addSensor(subsystem, "Boundary Limit Switch", boundaryLimit);

    encoder.setPIDSourceType(PIDSourceType.kRate);
    pid = new PIDSpeedController(encoder, motor, subsystem, "Speed Control");
    subsystemName = subsystem;
}

public SetDistanceToBox(double distance) {
    requires(Robot.drivetrain);
    pid = new PIDController(-2, 0, 0, new PIDSource() {
        PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

        public double pidGet() {
            return Robot.drivetrain.getDistanceToObstacle();
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            m_sourceType = pidSource;
        }

        @Override
        public PIDSourceType getPIDSourceType() {
            return m_sourceType;
        }
    }, new PIDOutput() {
        public void pidWrite(double d) {
            Robot.drivetrain.drive(d, d);
        }
    });
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
}

public DriveStraight(double distance) {
    requires(Robot.drivetrain);
    pid = new PIDController(4, 0, 0,
            new PIDSource() {
                PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

                public double pidGet() {
                    return Robot.drivetrain.getDistance();
                }

                @Override
                public void setPIDSourceType(PIDSourceType pidSource) {
                  m_sourceType = pidSource;
                }

                @Override
                public PIDSourceType getPIDSourceType() {
                    return m_sourceType;
                }
            },
            new PIDOutput() { public void pidWrite(double d) {
                Robot.drivetrain.drive(d, d);
            }});
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
}

protected static PIDSource sourceFor(SystemModel model, PIDSourceType initialSourceType) {
    return new PIDSource() {
        private PIDSourceType sourceType = initialSourceType;

        @Override
        public PIDSourceType getPIDSourceType() {
            return sourceType;
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            this.sourceType = pidSource;
        }

        @Override
        public double pidGet() {
            return model.getActualValue();
        }
    };
}

public DriveToPeg(){        
    double distance = .2;

    requires(Robot.driveBase);
    double kP = -.4;
    double kI = 1;
    double kD = 5;

    pid = new PIDController(kP, kI, kD, new PIDSource() {
        PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

        @Override
        public double pidGet() {
            return Robot.driveBase.getDistanceAhead();
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            m_sourceType = pidSource;
        }

        @Override
        public PIDSourceType getPIDSourceType() {
            return m_sourceType;
        }
    }, new PIDOutput() {
        @Override
        public void pidWrite(double d) {
            Robot.driveBase.driveForward(d);
            System.out.println(d);
        }
    });
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
    pid.setOutputRange(0, .35);

    SmartDashboard.putData("driveToPeg", pid);
}

public Drive() {
    initializeGear();
    // setPIDConstants();
    // leftEncoder.setDistancePerPulse(distancePerPulse);
    // rightEncoder.setDistancePerPulse(distancePerPulse);
    leftEncoder.setPIDSourceType(PIDSourceType.kRate);
    rightEncoder.setPIDSourceType(PIDSourceType.kRate);
}

public AimShooter() {

        pot = new AnalogInput(RobotMap.Analog.ShooterPotAngle);
        pot.setPIDSourceType(PIDSourceType.kDisplacement);
        motor = new Victor(RobotMap.Pwm.ShooterAngleMotor);

        anglePID = new PIDSpeedController(pot, motor, "anglePID", "AimShooter");

        updatePIDConstants();
        anglePID.set(0);
    }

/**
 * Returns the current yaw value reported by the sensor.  This
 * yaw value is useful for implementing features including "auto rotate 
 * to a known angle".
 * @return The current yaw angle in degrees (-180 to 180).
 */
public double pidGet() {
    if ( pid_source_type == PIDSourceType.kRate ) {
        return getRate();
    } else {
        return getYaw();
    }
}

/**
 * Returns the current yaw value reported by the sensor.  This
 * yaw value is useful for implementing features including "auto rotate 
 * to a known angle".
 * @return The current yaw angle in degrees (-180 to 180).
 */
public double pidGet() {
    if ( pid_source_type == PIDSourceType.kRate ) {
        return getRate();
    } else {
        return getYaw();
    }
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) 
{

}

@Override
public PIDSourceType getPIDSourceType() 
{
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
    pidSourceType = pidSource;
}

@Override
public PIDSourceType getPIDSourceType() {
    return pidSourceType;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
    pidSourceType = pidSource;
}

@Override
public PIDSourceType getPIDSourceType() {
    return pidSourceType;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
}

@Override
public PIDSourceType getPIDSourceType() {
    return PIDSourceType.kDisplacement;
}

@Override
public PIDSourceType getPIDSourceType() {
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {}

@Override
public PIDSourceType getPIDSourceType() {
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
    m_pidSource.setPIDSourceType(pidSource);
}

@Override
public PIDSourceType getPIDSourceType() {
    return m_pidSource.getPIDSourceType();
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
    // TODO Auto-generated method stub

}

@Override
public PIDSourceType getPIDSourceType() {
    // TODO Auto-generated method stub
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
    // TODO Auto-generated method stub

}

@Override
public PIDSourceType getPIDSourceType() {
    // TODO Auto-generated method stub
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
}

@Override
public PIDSourceType getPIDSourceType() {
    // TODO Auto-generated method stub
    return PIDSourceType.kDisplacement;
}

public void setPIDSourceType(PIDSourceType pidSource)
{
}

public PIDSourceType getPIDSourceType()
{
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
}

@Override
public PIDSourceType getPIDSourceType() {
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
    // TODO Auto-generated method stub

}

@Override
public PIDSourceType getPIDSourceType() {
    // TODO Auto-generated method stub
    return PIDSourceType.kDisplacement;
}

@Override
public void setPIDSourceType(PIDSourceType pidSource) {
}

@Override
public PIDSourceType getPIDSourceType() {
    return PIDSourceType.kDisplacement;
}