From d0b0d55dc7137f7725c67edca8bc2f322ea8b170 Mon Sep 17 00:00:00 2001 From: EvanYap Date: Sat, 4 Feb 2017 16:28:40 -0800 Subject: [PATCH] save in case my comp crashes --- .../usfirst/frc/team3501/robot/Constants.java | 16 ++++----- src/org/usfirst/frc/team3501/robot/OI.java | 1 + src/org/usfirst/frc/team3501/robot/Robot.java | 34 +++++++++++++++++-- .../team3501/robot/utils/PIDController.java | 9 +++++ 4 files changed, 49 insertions(+), 11 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 47b8959..89a5ba0 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -57,15 +57,15 @@ public class Constants { public static final int ENCODER_RIGHT_B = 3; // PID TUNING - public static final String DRIVE_P_Val = "P"; - public static final String DRIVE_I_Val = "I"; - public static final String DRIVE_D_Val = "D"; - public static final String DRIVE_TARGET_DIST = "SETPOINT"; + 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_TARGET_DIST = "SET_DIST"; public static final String DRIVE_MOTOR_VAL = "SPEED"; - public static final String GYRO_P_Val = "P"; - public static final String GYRO_I_Val = "I"; - public static final String GYRO_D_Val = "D"; - public static final String GYRO_TARGET_ANGLE = "SETVALUE"; + public static final String GYRO_P_Val = "GyroP"; + public static final String GYRO_I_Val = "GyroI"; + public static final String GYRO_D_Val = "GyroD"; + public static final String GYRO_TARGET_ANGLE = "SET_ANGLE"; public static final int PID_ERROR = -1; public static final int TARGET_DISTANCE_ERROR = -1; diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index 510b722..0018600 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -50,6 +50,7 @@ public class OI { toggleGear = new JoystickButton(leftJoystick, Constants.OI.TOGGLE_GEAR_PORT); + toggleGear.whenPressed(new ToggleGear()); runIntake = new JoystickButton(leftJoystick, diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 90d2305..6dd5950 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -16,7 +16,7 @@ public class Robot extends IterativeRobot { @Override public void robotInit() { - driveTrain = driveTrain; + driveTrain = DriveTrain.getDriveTrain(); oi = OI.getOI(); shooter = Shooter.getShooter(); intake = Intake.getIntake(); @@ -53,6 +53,32 @@ public class Robot extends IterativeRobot { SmartDashboard.putNumber(Constants.DriveTrain.GYRO_P_Val, 0); SmartDashboard.putNumber(Constants.DriveTrain.GYRO_I_Val, 0); SmartDashboard.putNumber(Constants.DriveTrain.GYRO_D_Val, 0); + + double driveP = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val, + Constants.DriveTrain.PID_ERROR); + double driveI = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_I_Val, + Constants.DriveTrain.PID_ERROR); + double driveD = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val, + Constants.DriveTrain.PID_ERROR); + + double gyroP = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_P_Val, + Constants.DriveTrain.PID_ERROR); + double gyroI = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_I_Val, + Constants.DriveTrain.PID_ERROR); + double gyroD = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_D_Val, + Constants.DriveTrain.PID_ERROR); + + double Setpoint = SmartDashboard.getNumber( + Constants.DriveTrain.DRIVE_TARGET_DIST, Constants.DriveTrain.PID_ERROR); + double Speed = SmartDashboard.getNumber( + Constants.DriveTrain.DRIVE_MOTOR_VAL, Constants.DriveTrain.PID_ERROR); + + driveTrain.getDriveController().setName("Drive"); + driveTrain.getGyroController().setName("Gyro"); + + driveTrain.getDriveController().setConstants(driveP, driveI, driveD); + + driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD); } @Override @@ -73,8 +99,10 @@ public class Robot extends IterativeRobot { double gyroD = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_D_Val, Constants.DriveTrain.PID_ERROR); - DriveTrain.getDriveTrain().getDriveController().setConstants(driveP, driveI, - driveD); + driveTrain.getDriveController().setName("Drive"); + driveTrain.getGyroController().setName("Gyro"); + + driveTrain.getDriveController().setConstants(driveP, driveI, driveD); driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD); diff --git a/src/org/usfirst/frc/team3501/robot/utils/PIDController.java b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java index 073eee2..3560dcb 100644 --- a/src/org/usfirst/frc/team3501/robot/utils/PIDController.java +++ b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java @@ -18,6 +18,8 @@ public class PIDController { private int minDoneCycleCount; private int doneCycleCount; + private String name; + public PIDController() { this(0.0, 0.0, 0.0, 0.0, 0.0); } @@ -42,6 +44,10 @@ public class PIDController { } + public void setName(String s) { + this.name = s; + } + public PIDController(double p, double i, double d, double eps) { this(p, i, d, eps, eps * 0.8); } @@ -54,6 +60,9 @@ public class PIDController { this.pConst = p; this.iConst = i; this.dConst = d; + + System.out.println(this.name + " " + " P: " + this.pConst + " I: " + + this.iConst + " D: " + this.dConst); } public void setConstants(double p, double i, double d, double eps, -- 2.30.2