shooter = Shooter.getShooter();
intake = Intake.getIntake();
- SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0);
- SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0);
- SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, 0);
- SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0);
+ SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0.006);
+ SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0.0011);
+ SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, -0.002);
+ SmartDashboard.putNumber(driveTrain.DRIVE_GYRO_P_Val, 0.01);
SmartDashboard.putNumber(driveTrain.DRIVE_TARGET_DIST, 50);
SmartDashboard.putNumber(driveTrain.MAX_TIME_OUT, 10);
// both set to high gear
@Override
public void autonomousInit() {
+ System.out.println("AUTON INIT");
driveTrain.setHighGear();
double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_P_Val,
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();
-
+ SmartDashboard.putNumber("angle", driveTrain.getAngle());
}
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
this.driveController.setSetPoint(this.target);
+ zeroAngle = driveTrain.getAngle();
}
@Override
protected void execute() {
+ System.out.printf("angle: %.2f\t", driveTrain.getAngle() - zeroAngle);
double xVal = driveStraightGyroP * (driveTrain.getAngle() - zeroAngle);
double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
double rightDrive = yVal + xVal;
this.driveTrain.setMotorValues(leftDrive, rightDrive);
- System.out.println(
- "PID Vals: " + driveController.getP() + " " + driveController.getI()
- + " " + driveController.getD());
+ // SmartDashboard.putNumber("x", xVal);
+ // SmartDashboard.putNumber("y", yVal);
+ //
+ // SmartDashboard.putNumber("left", driveTrain.getLeftEncoderDistance());
+ // SmartDashboard.putNumber("right", driveTrain.getRightEncoderDistance());
+ //
+ // System.out.printf("x: %.2f\t", xVal);
+ // System.out.printf("y: %.2f\t", yVal);
+ //
+ // System.out.printf("dist: %.2f\n", driveTrain.getAvgEncoderDistance());
- System.out.println("Encoder " + driveTrain.getAvgEncoderDistance());
}
@Override
* Changes the gear to a DoubleSolenoid.Value
*/
private void changeGear(DoubleSolenoid.Value gear) {
+ System.out.println("shifting to " + gear);
leftGearPiston.set(gear);
rightGearPiston.set(gear);
}