From 63c60c71b8c48eaffa8607cd6167e3357c0a3ba4 Mon Sep 17 00:00:00 2001 From: Michael Wang Date: Thu, 9 Feb 2017 19:54:39 -0800 Subject: [PATCH] fix errors --- src/org/usfirst/frc/team3501/robot/Robot.java | 67 +++++++++++-------- .../team3501/robot/subsystems/DriveTrain.java | 14 ++++ 2 files changed, 54 insertions(+), 27 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 7860f1e..b910b29 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -21,15 +21,15 @@ public class Robot extends IterativeRobot { shooter = Shooter.getShooter(); intake = Intake.getIntake(); - SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_P_Val, 0); - SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_I_Val, 0); - SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_D_Val, 0); - SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_TARGET_DIST, 50); - SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_MOTOR_VAL, 0.5); + 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_TARGET_DIST, 50); + SmartDashboard.putNumber(driveTrain.DRIVE_MOTOR_VAL, 0.5); - SmartDashboard.putNumber(Constants.DriveTrain.GYRO_P_Val, 0); - SmartDashboard.putNumber(Constants.DriveTrain.GYRO_I_Val, 0); - SmartDashboard.putNumber(Constants.DriveTrain.GYRO_D_Val, 0); + SmartDashboard.putNumber(driveTrain.GYRO_P_Val, 0); + SmartDashboard.putNumber(driveTrain.GYRO_I_Val, 0); + SmartDashboard.putNumber(driveTrain.GYRO_D_Val, 0); } @@ -55,6 +55,7 @@ public class Robot extends IterativeRobot { public void autonomousInit() { driveTrain.setHighGear(); +<<<<<<< HEAD SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_P_Val, 0); SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_I_Val, 0); SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_D_Val, 0); @@ -68,25 +69,37 @@ public class Robot extends IterativeRobot { SmartDashboard.putNumber(Constants.DriveTrain.MOTOR_VAL, 0.5); SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_MOTOR_VAL, 0.5); - - 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); +======= + 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_TARGET_DIST, 50); + + SmartDashboard.putNumber(driveTrain.DRIVE_MOTOR_VAL, 0.5); + + SmartDashboard.putNumber(driveTrain.GYRO_P_Val, 0); + SmartDashboard.putNumber(driveTrain.GYRO_I_Val, 0); + SmartDashboard.putNumber(driveTrain.GYRO_D_Val, 0); +>>>>>>> fix errors + + double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_D_Val, + driveTrain.PID_ERROR); + double driveI = SmartDashboard.getNumber(driveTrain.DRIVE_I_Val, + driveTrain.PID_ERROR); + double driveD = SmartDashboard.getNumber(driveTrain.DRIVE_D_Val, + driveTrain.PID_ERROR); + + double gyroP = SmartDashboard.getNumber(driveTrain.GYRO_P_Val, + driveTrain.PID_ERROR); + double gyroI = SmartDashboard.getNumber(driveTrain.GYRO_I_Val, + driveTrain.PID_ERROR); + double gyroD = SmartDashboard.getNumber(driveTrain.GYRO_D_Val, + driveTrain.PID_ERROR); + + double Setpoint = SmartDashboard.getNumber(driveTrain.DRIVE_TARGET_DIST, + driveTrain.PID_ERROR); + double Speed = SmartDashboard.getNumber(driveTrain.DRIVE_MOTOR_VAL, + driveTrain.PID_ERROR); driveTrain.getDriveController().setName("Drive"); driveTrain.getGyroController().setName("Gyro"); diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index dbe14ac..6d4f020 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -48,6 +48,20 @@ public class DriveTrain extends Subsystem { driveController = new PIDController(driveP, driveI, driveD); gyroController = new PIDController(turnP, turnI, turnD); + // PID TUNING + + 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 = "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; + // MOTOR CONTROLLERS frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); -- 2.30.2