X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=fd6530e5404d26a08985c7abe5df66f555e69af6;hp=54324f050caab86d9ac9870ca11dace772b6a63c;hb=9b35aa55bb9b437483e2886f9cf56f0812e19094;hpb=6610a8074622d61a63f6afd8dfe7121672bcb4c5 diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 54324f0..fd6530e 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -2,6 +2,7 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; +import org.usfirst.frc.team3501.robot.utils.BNO055; import com.ctre.CANTalon; @@ -16,6 +17,9 @@ 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; + public static double driveP = 0.008, driveI = 0.001, driveD = -0.002; + public static double defaultGyroP = 0.009, defaultGyroI = 0.00000, + defaultGyroD = -0.000; private double gyroZero = 0; public static final double WHEEL_DIAMETER = 6; // inches @@ -30,6 +34,7 @@ public class DriveTrain extends Subsystem { private final DoubleSolenoid leftGearPiston, rightGearPiston; private ADXRS450_Gyro imu; + private BNO055 imu; private DriveTrain() { // MOTOR CONTROLLERS @@ -177,6 +182,21 @@ public class DriveTrain extends Subsystem { rightGearPiston.set(gear); } + public double getAngle() { + if (!this.imu.isInitialized()) + return -1; + return this.imu.getHeading() - this.gyroZero; + } + + public void resetGyro() { + this.gyroZero = this.getAngle(); + + } + + public double getZeroAngle() { + return this.gyroZero; + } + @Override protected void initDefaultCommand() { setDefaultCommand(new JoystickDrive());