From: Cindy Zhang Date: Fri, 3 Feb 2017 04:19:36 +0000 (-0800) Subject: change some names X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=c846487e0cc034e05c44e3a2a11c9c72fde1bee9 change some names --- diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java index f9f5c7f..867f5ed 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java @@ -37,9 +37,9 @@ public class TurnForAngle extends Command { this.target = Math.abs(angle); this.zeroAngle = driveTrain.getAngle(); - this.gyroP = driveTrain.defaultGyroP; - this.gyroI = driveTrain.defaultGyroI; - this.gyroD = driveTrain.defaultGyroD; + this.gyroP = driveTrain.turnP; + this.gyroI = driveTrain.turnI; + this.gyroD = driveTrain.turnD; this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD); this.gyroController.setDoneRange(1); diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 645e345..f7c35a9 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -14,9 +14,8 @@ import edu.wpi.first.wpilibj.command.Subsystem; public class DriveTrain extends Subsystem { public static double driveP = 0.006, driveI = 0.001, driveD = -0.002; - public static double defaultGyroP = 0.004, defaultGyroI = 0.0013, - defaultGyroD = -0.005; - private double gyroZero = 0; + public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005; + public static double driveStraightGyroP = 0.01; public static final double WHEEL_DIAMETER = 6; // inches public static final int ENCODER_PULSES_PER_REVOLUTION = 256; @@ -106,11 +105,9 @@ public class DriveTrain extends Subsystem { } public void printEncoderOutput() { - // System.out.println("left: " + getLeftEncoderDistance()); - // System.out.println("right: " + getRightEncoderDistance()); - System.out.println(getAvgEncoderDistance()); System.out.println("left: " + getLeftEncoderDistance()); System.out.println("right: " + getRightEncoderDistance()); + // System.out.println(getAvgEncoderDistance()); } public double getAvgEncoderDistance() { @@ -136,17 +133,13 @@ public class DriveTrain extends Subsystem { // ------Gyro------// public double getAngle() { - return this.imu.getAngle() - this.gyroZero; + return this.imu.getAngle(); } public void resetGyro() { this.imu.reset(); } - public double getZeroAngle() { - return this.gyroZero; - } - /* * @return a value that is the current setpoint for the piston kReverse or * KForward