package org.usfirst.frc.team3501.robot;
-import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
-import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch;
import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
Constants.OI.DECREASE_SHOOTER_SPEED_PORT);
decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed());
- if (!isClimbing) {
- toggleWinch.whenPressed(new RunWinchContinuous());
- isClimbing = true;
- } else {
- toggleWinch.whenPressed(new MaintainClimbedPosition());
- isClimbing = false;
- }
-
/*
* if (!Robot.getDriveTrain().isClimbing()) { toggleWinch.whenPressed(new
* RunWinchContinuous()); Robot.getDriveTrain().setClimbing(true); } else {
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_TARGET_DIST, 50);
SmartDashboard.putNumber(driveTrain.MAX_TIME_OUT, 10);
driveTrain.getDriveController().setName("Drive");
driveTrain.getGyroController().setName("Gyro");
- driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
+ driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
- Scheduler.getInstance().add(new DriveDistance(setpoint, maxTimeOut));
+ Scheduler.getInstance()
+ .add(new DriveDistance(setpoint, maxTimeOut));
}
@Override
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This command makes the robot drive a specified distance using encoders on the
private Preferences prefs;
private PIDController driveController;
- private double gyroP;
+ private double driveStraightGyroP;
public DriveDistance(double distance, double maxTimeOut) {
requires(driveTrain);
this.target = distance;
+ driveStraightGyroP = SmartDashboard.getNumber(driveTrain.DRIVE_GYRO_P_Val,
+ driveTrain.PID_ERROR);
this.driveController = driveTrain.getDriveController();
this.driveController.setDoneRange(0.5);
@Override
protected void execute() {
- double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
+ double xVal = driveStraightGyroP * (driveTrain.getAngle() - zeroAngle);
double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
double leftDrive = yVal - xVal;
double rightDrive = yVal + xVal;
this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+ System.out.println(
+ "PID VALS: " + driveController.getP() + " " + driveController.getI()
+ + " " + driveController.getD());
+
+ System.out.println(driveTrain.getAvgEncoderDistance());
}
@Override
public static final String DRIVE_P_Val = "DriveP";
public static final String DRIVE_I_Val = "DriveI";
public static final String DRIVE_D_Val = "DriveD";
+ public static final String DRIVE_GYRO_P_Val = "DriveGyroP";
public static final String DRIVE_TARGET_DIST = "SET_DIST";
public static final String MAX_TIME_OUT = "MaxTimeOut";
public static final String GYRO_P_Val = "GyroP";
public static final int PID_ERROR = -1;
public static final int TARGET_DISTANCE_ERROR = -1;
- public static final double WHEEL_DIAMETER = 6; // inches
+ public static final double WHEEL_DIAMETER = 4; // inches
public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
/ ENCODER_PULSES_PER_REVOLUTION;