From 0bceba2141a6acb0bdb49f7b999724b03470e248 Mon Sep 17 00:00:00 2001 From: Declan Freeman-Gleason Date: Sat, 9 Jun 2018 13:49:20 -0700 Subject: [PATCH] Fix throttle and straight driving. We're shipping! --- src/main/java/com/spartronics4915/atlas/OI.java | 5 +++++ src/main/java/com/spartronics4915/atlas/Robot.java | 3 +-- .../atlas/commands/DriveStraightDrivetrain.java | 11 +++++------ .../atlas/commands/QuickTurnDrivetrain.java | 3 ++- .../atlas/commands/TeleopDrivetrain.java | 2 +- .../atlas/subsystems/Drivetrain.java | 13 ++++++++++--- 6 files changed, 24 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/spartronics4915/atlas/OI.java b/src/main/java/com/spartronics4915/atlas/OI.java index 8bae246..f77d035 100644 --- a/src/main/java/com/spartronics4915/atlas/OI.java +++ b/src/main/java/com/spartronics4915/atlas/OI.java @@ -153,4 +153,9 @@ private void initDrivetrainOI() // Initalize the drivetrain } + public static double getScaledThrottle() + { + return Math.max(Math.min(1-OI.sDriveStick.getZ(),1),0.4); + } + } diff --git a/src/main/java/com/spartronics4915/atlas/Robot.java b/src/main/java/com/spartronics4915/atlas/Robot.java index 362ea2e..9f9f8cc 100644 --- a/src/main/java/com/spartronics4915/atlas/Robot.java +++ b/src/main/java/com/spartronics4915/atlas/Robot.java @@ -46,8 +46,7 @@ public void robotInit() @Override public void robotPeriodic() { - // This is invoked in both Autonomous and TeleOp, in - // addition to the autonomousPeriodic and TeleOp periodic. + SmartDashboard.putNumber("IMU Heading", mDrivetrain.getIMUHeading().getDegrees()); } @Override diff --git a/src/main/java/com/spartronics4915/atlas/commands/DriveStraightDrivetrain.java b/src/main/java/com/spartronics4915/atlas/commands/DriveStraightDrivetrain.java index 5e0b661..f366850 100644 --- a/src/main/java/com/spartronics4915/atlas/commands/DriveStraightDrivetrain.java +++ b/src/main/java/com/spartronics4915/atlas/commands/DriveStraightDrivetrain.java @@ -18,9 +18,9 @@ public class DriveStraightDrivetrain extends Command implements PIDSource, PIDOutput { - private static final double kP = 0.5; - private static final double kI = 0; - private static final double kD = 10; + private static final double kP = 0; // Was 0.4 + private static final double kI = 0.2; + private static final double kD = 0; private static final double kF = 0; private static final double kAllowedError = 0; // In degrees @@ -55,8 +55,7 @@ protected void initialize() protected void execute() { mPIDController.enable(); - - SmartDashboard.putNumber("IMU Heading", mDrivetrain.getIMUHeading().getDegrees()); + SmartDashboard.putNumber("Error", mPIDController.getError()); } @Override @@ -119,6 +118,6 @@ public double pidGet() @Override public void pidWrite(double output) { - mDrivetrain.arcadeDrive(OI.sDriveStick.getZ(), output); + mDrivetrain.arcadeDrive(-1*OI.getScaledThrottle(), output); } } diff --git a/src/main/java/com/spartronics4915/atlas/commands/QuickTurnDrivetrain.java b/src/main/java/com/spartronics4915/atlas/commands/QuickTurnDrivetrain.java index cd63df4..ab31c83 100644 --- a/src/main/java/com/spartronics4915/atlas/commands/QuickTurnDrivetrain.java +++ b/src/main/java/com/spartronics4915/atlas/commands/QuickTurnDrivetrain.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * QuickTurnDrivetrain turns the drivetrain 180 degrees in a closed @@ -17,7 +18,7 @@ public class QuickTurnDrivetrain extends Command implements PIDSource, PIDOutput { - private static final double kP = 0.01; + private static final double kP = 0.02; private static final double kI = 0; private static final double kD = 0; private static final double kF = 0; diff --git a/src/main/java/com/spartronics4915/atlas/commands/TeleopDrivetrain.java b/src/main/java/com/spartronics4915/atlas/commands/TeleopDrivetrain.java index faf934f..75a6d56 100644 --- a/src/main/java/com/spartronics4915/atlas/commands/TeleopDrivetrain.java +++ b/src/main/java/com/spartronics4915/atlas/commands/TeleopDrivetrain.java @@ -31,7 +31,7 @@ protected void initialize() protected void execute() { // getZ is the throttle - mDrivetrain.arcadeDrive(OI.sDriveStick.getY()*Math.min(1-OI.sDriveStick.getZ(),1), OI.sDriveStick.getX()*-1); // Steering is reversed! + mDrivetrain.arcadeDrive(OI.sDriveStick.getY()*OI.getScaledThrottle(), OI.sDriveStick.getX()*-1); // Steering is reversed! } @Override diff --git a/src/main/java/com/spartronics4915/atlas/subsystems/Drivetrain.java b/src/main/java/com/spartronics4915/atlas/subsystems/Drivetrain.java index 4a80ac9..b33e94e 100644 --- a/src/main/java/com/spartronics4915/atlas/subsystems/Drivetrain.java +++ b/src/main/java/com/spartronics4915/atlas/subsystems/Drivetrain.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * The subsystem that controls the Drivetrain. @@ -28,6 +29,9 @@ public class Drivetrain extends SpartronicsSubsystem private DifferentialDrive mDifferentialDrive; + // CTRE does a pass-by-reference thing, so we might as well not allocate a new array every time + private double[] yawPitchRoll = new double[3]; + // Motors public Victor mLeftMotor; public Victor mRightMotor; @@ -58,6 +62,9 @@ private Drivetrain() //Initialize IMU mIMU = new PigeonIMU(RobotMap.kDriveTrainIMUID); + // Do a song and dance to make the IMU work + mIMU.clearStickyFaults(0); // 0 timeout means no timeout + // Setup the differential drive mDifferentialDrive = new DifferentialDrive(mLeftMotor, mRightMotor); @@ -87,9 +94,9 @@ public void stop() public Rotation2d getIMUHeading() { - double[] ypr = new double[3]; - mIMU.getYawPitchRoll(ypr); - return Rotation2d.fromDegrees(ypr[0]); + mIMU.getYawPitchRoll(yawPitchRoll); + SmartDashboard.putString("YawPitchRoll", "[0]: "+yawPitchRoll[0]+"; [1]: "+yawPitchRoll[1]+"; [2]: "+yawPitchRoll[2]+"; Fused: "+mIMU.getFusedHeading()); + return Rotation2d.fromDegrees(yawPitchRoll[0]); } public void arcadeDrive(double speed, double rotation)