diff --git a/build.gradle b/build.gradle index 6ae9dcf..e00a044 100644 --- a/build.gradle +++ b/build.gradle @@ -11,8 +11,7 @@ def ROBOT_MAIN_CLASS = "frc4388.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project EmbeddedTools. deploy { - targets { - roboRIO("roborio") { + targets { roboRIO("roborio") { // Team number is loaded either from the .wpilib/wpilib_preferences.json // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 8c15e6a..41328b4 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -97,7 +97,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); + m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99b7d52..86be53e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc4388.robot.commands.DistanceElevatorPID; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; +import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; @@ -57,18 +58,13 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.driveWithInput( - getDriverController().getLeftYAxis(), - getDriverController().getRightXAxis()), m_robotDrive)); - - // drives motor with input from triggers on the operator controller + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); + // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // moves elevator with one-axis input from the driver controller m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( getOperatorController().getLeftYAxis()), m_robotElevator )); - // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java new file mode 100644 index 0000000..2b31457 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; +import frc4388.utility.controller.IHandController; + +public class DriveWithJoystick extends CommandBase { + private Drive m_drive; + private IHandController m_controller; + + /** + * Creates a new DriveWithJoystick. + */ + public DriveWithJoystick(Drive subsystem, IHandController controller) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_controller = controller; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double moveInput = m_controller.getLeftYAxis(); + double steerInput = m_controller.getRightXAxis(); + double moveOutput = 0; + double steerOutput = 0; + if (moveInput >= 0){ + moveOutput = -Math.cos(1.571*moveInput)+1; + } else { + moveOutput = Math.cos(1.571*moveInput)-1; + } + + double cosMultiplier = .45; + double deadzone = .2; + if (steerInput > 0){ + steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); + } else { + steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + } + + m_drive.driveWithInput(moveOutput, steerOutput); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index df272f9..9ccb252 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -55,6 +55,9 @@ public class Drive extends SubsystemBase { /* set neutral mode */ setDriveTrainNeutralMode(NeutralMode.Brake); + m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE + m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed + /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(false);