public DriveTrain() { // TODO: would be nice to migrate stuff from RobotMap here. // m_turnPID is used to improve accuracy during auto-turn operations. m_imu = new IMUPIDSource(); m_turnPID = new PIDController(turnKp, turnKi, turnKd, turnKf, m_imu, new PIDOutput() { public void pidWrite(double output) { // output is [-1, 1]... we need to // convert this to a speed... Robot.driveTrain.turn(output * MAXIMUM_TURN_SPEED); // robotDrive.tankDrive(-output, output); } }); m_turnPID.setOutputRange(-1, 1); m_turnPID.setInputRange(-180, 180); m_turnPID.setPercentTolerance(2); // m_turnPID.setContuous? }
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); }
/** * Creates a new {@link MecanumDriveAlgorithm} that controls the specified * {@link FourWheelDriveController}. * * @param controller the {@link FourWheelDriveController} to control * @param gyro the {@link Gyro} to use for orientation correction and * field-oriented driving */ public MecanumDriveAlgorithm(FourWheelDriveController controller, Gyro gyro) { super(controller); // Necessary because we hide the controller field inherited from // DriveAlgorithm (if this was >=Java 5 I would use generics). this.controller = controller; this.gyro = gyro; rotationPIDController = new PIDController( ROTATION_P, ROTATION_I, ROTATION_D, ROTATION_F, gyro, new PIDOutput() { public void pidWrite(double output) { rotationSpeedPID = output; } } ); SmartDashboard.putData("Rotation PID Controller", rotationPIDController); }
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 BangBangController (PIDOutput pidOutput, PIDSource pidSource, double period){ _pidSource = pidSource; _pidOutput = pidOutput; _period = period; _setPoint = 0; _timer.schedule(new _bgTask(), 0, (long) (1000*_period)); }
public TakeBackHalfPlusPlus(PIDOutput pidoutput, PIDSource pidsource, double period, double max, double min){ _pidSource = pidsource; _pidOutput = pidoutput; _setPoint = 0; _period = period; _max = max; _min = min; _timer.schedule(new _bgTask(), 0, (long) (1000 * _period)); }
/** * Allocate a PID object with the given constants for P, I, D, and F * * @param Kp the proportional coefficient * @param Ki the integral coefficient * @param Kd the derivative coefficient * @param Kf the feed forward term * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output percentage * @param period the loop time for doing calculations. This particularly * effects calculations of the integral and differential terms. The default * is 50ms. */ public PIDController649(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output, double period) { if (source == null) { throw new NullPointerException("Null PIDSource was given"); } if (output == null) { throw new NullPointerException("Null PIDOutput was given"); } m_controlLoop = new java.util.Timer(); m_P = Kp; m_I = Ki; m_D = Kd; m_F = Kf; m_pidInput = source; m_pidOutput = output; m_period = period; m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000)); instances++; UsageReporting.report(UsageReporting.kResourceType_PIDController, instances); m_tolerance = new NullTolerance(); m_outputDirection = true; }
private Tilter() { leadscrew = new BoundedTalon(Constants.TILTER_CHANNEL, Constants.TILTER_UPPER_LIMIT_SWITCH_CHANNEL, Constants.TILTER_LOWER_LIMIT_SWITCH_CHANNEL); accel = new ADXL345_I2C(Constants.ACCELEROMETER_CHANNEL, ADXL345_I2C.DataFormat_Range.k2G); accelMeasurements = new Vector(); updateAccel(); pendulum = new AnalogChannel(Constants.PENDULUM_CHANNEL); enc = new Encoder(Constants.LEADSCREW_ENCODER_A_CHANNEL, Constants.LEADSCREW_ENCODER_B_CHANNEL); enc.setDistancePerPulse(Constants.TILTER_DISTANCE_PER_PULSE); enc.start(); Thread t = new Thread(new Runnable() { public void run() { Tilter.net = new NetworkIO(); } }); t.start(); controller = new PIDController(Constants.PVAL_T, Constants.IVAL_T, Constants.DVAL_T, enc, new PIDOutput() { public void pidWrite(double output) { setLeadscrewMotor(output); } }); controller.setPercentTolerance(0.01); controller.disable(); updatePID(); initialReadingTimer = new Timer(); initialReadingTimer.schedule(new TimerTask() { public void run() { initialLeadLength = calcLeadscrewLength(getAveragedAccelBasedAngle()); } }, INITIAL_ANGLE_MEASUREMENT_DELAY); }
public RateLimitedPIDOutput(double rate, PIDOutput output) { this.maxRate = rate; this.output = output; }
public DeltaPID(IMUAdvanced source, PIDOutput output) { super(velP,velI,velD, 0, source, output); prevYaw=RobotMap.imu.getYaw(); }
public PIDSpeedController(PIDSource source, PIDOutput output, String subsystem, String controllerName) { controller = new PIDController(0, 0, 0, 0, source, output); LiveWindow.addActuator(subsystem, controllerName, controller); }
/** * Read the input, calculate the output accordingly, and write to the * output. This should only be called by the PIDTask and is created during * initialization. */ private void calculate() { boolean enabled; PIDSource pidInput; synchronized (this) { if (m_pidInput == null) { return; } if (m_pidOutput == null) { return; } enabled = m_enabled; // take snapshot of these values... pidInput = m_pidInput; } if (enabled) { double input; input = pidInput.pidGet(); double result; PIDOutput pidOutput = null; synchronized (this) { m_error = m_setpoint - input; if (m_continuous) { if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) { if (m_error > 0) { m_error = m_error - m_maximumInput + m_minimumInput; } else { m_error = m_error + m_maximumInput - m_minimumInput; } } } if (m_I != 0) { double potentialIGain = (m_totalError + m_error) * m_I; if (potentialIGain < m_maximumOutput) { if (potentialIGain > m_minimumOutput) { m_totalError += m_error; } else { m_totalError = m_minimumOutput / m_I; } } else { m_totalError = m_maximumOutput / m_I; } } //****************************************************************************************************************** if (m_error > 0 && m_prevError < 0 || m_error < 0 && m_prevError > 0) { m_totalError = 0; } //****************************************************************************************************************** m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F; m_errorDiff = m_prevError - m_error; m_prevError = m_error; if (m_result > m_maximumOutput) { m_result = m_maximumOutput; } else if (m_result < m_minimumOutput) { m_result = m_minimumOutput; } pidOutput = m_pidOutput; result = m_result * (m_outputDirection ? 1 : -1); } pidOutput.pidWrite(result); } }
/** * Creates a {@link PIDController} from a {@link PIDConstants} object and * the specified source and output. * * @param constants the PID constants * @param source the PID source * @param output the PID output * @return a shiny new PID controller */ public static PIDController createPIDControllerFromConstants(PIDConstants constants, PIDSource source, PIDOutput output) { return new PIDController(constants.p, constants.i, constants.d, constants.f, source, output); }
/** * Allocate a PID object with the given constants for P, I, D and period * * @param Kp * @param Ki * @param Kd * @param source * @param output * @param period */ public PIDController649(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) { this(Kp, Ki, Kd, 0.0, source, output, period); }
/** * Allocate a PID object with the given constants for P, I, D, using a 50ms * period. * * @param Kp the proportional coefficient * @param Ki the integral coefficient * @param Kd the derivative coefficient * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output percentage */ public PIDController649(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) { this(Kp, Ki, Kd, source, output, kDefaultPeriod); }
/** * Allocate a PID object with the given constants for P, I, D, using a 50ms * period. * * @param Kp the proportional coefficient * @param Ki the integral coefficient * @param Kd the derivative coefficient * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output percentage */ public PIDController649(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) { this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod); }
