diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c0879e3..965062a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,7 +79,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // drives climber with input from triggers on the opperator controller @@ -195,8 +195,8 @@ public class RobotContainer { new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(1, 1), - new Translation2d(2, -1) + new Translation2d(3, 0) + //new Translation2d(2, -1) ), // End 3 meters straight ahead of where we started, facing forward new Pose2d(3, 0, new Rotation2d(0)), diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 64bb463..764cc61 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -66,7 +66,7 @@ public class Drive extends SubsystemBase { public DoubleSolenoid m_speedShift; public DoubleSolenoid m_coolFalcon; - public int m_currentTimeSec = (int)(System.currentTimeMillis() * 1000); + public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); public long m_lastTime, m_deltaTime; //in milliseconds @@ -84,7 +84,8 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); - m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()), new Pose2d(0, 0, new Rotation2d())); + m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), + new Pose2d(0, 0, new Rotation2d()) ); m_speedShift = new DoubleSolenoid(7,0,1); m_coolFalcon = new DoubleSolenoid(7,3,2); @@ -271,11 +272,15 @@ public class Drive extends SubsystemBase { @Override public void periodic() { + m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); + SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis()); if (m_currentTimeSec % 10 == 0) { coolFalcon(true); + SmartDashboard.putBoolean("Solenoid", true); } else if ((m_currentTimeSec - 2) % 10 == 0) { coolFalcon(false); + SmartDashboard.putBoolean("Solenoid", false); } m_deltaTime = System.currentTimeMillis() - m_lastTime; @@ -606,7 +611,7 @@ public class Drive extends SubsystemBase { * @param ticks The value in ticks to convert * @return The converted value in inches */ - public double ticksToInches(double ticks) { + public double ticksToInches(double ticks) { return ticks * DriveConstants.INCHES_PER_TICK; }