diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 662407e..730b1f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,7 +8,7 @@ public static final class DrivetrainConstants { public static final int DEVICE_ID_LEFT_FOLLOWER = 9; public static final int DEVICE_ID_RIGHT_LEADER = 3; public static final int DEVICE_ID_RIGHT_FOLLOWER = 4; - + public static final double SPEED_LIMIT = 0.4;//will be 0.5 } public static final class IntakeConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b68462c..0f11661 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,11 +34,12 @@ public void disabledExit() {} @Override public void autonomousInit() { + /* m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); - } + } */ } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c11005..96dddc7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.ClimbSubsystem; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.FlywheelSubsystem; @@ -28,31 +30,51 @@ public class RobotContainer { private final CommandJoystick driverRightJoystick = new CommandJoystick(PORT_NUMBER_DRIVER_RIGHT_JOYSTICK); private final CommandJoystick operatorJoystick = new CommandJoystick(PORT_NUMBER_OPERATOR_JOYSTICK); + //private boolean outreachMode = true; + //private double limit = 0.4; + public RobotContainer() { + /*SmartDashboard.putBoolean("outreachMode", outreachMode); + SmartDashboard.putNumber("Speed_Limit", limit); + + outreachMode = SmartDashboard.getBoolean("outreachMode", outreachMode); + limit = SmartDashboard.getNumber("Speed_Limit", limit);*/ + drivetrainSubsystem.setDefaultCommand(Commands.run( - () -> this.drivetrainSubsystem.arcadeDrive(driverLeftJoystick.getY(), driverRightJoystick.getX(), true), + () -> this.drivetrainSubsystem.arcadeDrive(driverLeftJoystick.getY(),driverLeftJoystick.getX()), this.drivetrainSubsystem)); - intakeSubsystem.setDefaultCommand(Commands.run(this.intakeSubsystem::stop, this.intakeSubsystem)); - flywheelSubsystem.setDefaultCommand(Commands.run(this.flywheelSubsystem::stop, this.flywheelSubsystem)); - climbSubsystem.setDefaultCommand( - Commands.run(() -> this.climbSubsystem.set(this.operatorJoystick.getY()), this.climbSubsystem)); + /*if(!outreachMode){ + //everything but drive taken out + intakeSubsystem.setDefaultCommand(Commands.run(this.intakeSubsystem::stop, this.intakeSubsystem)); + flywheelSubsystem.setDefaultCommand(Commands.run(this.flywheelSubsystem::stop, this.flywheelSubsystem)); + climbSubsystem.setDefaultCommand( + Commands.run(() -> this.climbSubsystem.set(this.operatorJoystick.getY()), this.climbSubsystem)); + }*/ configureBindings(); } private void configureBindings() { - driverLeftJoystick.button(BUTTON_ID_TRIGGER) - .whileTrue(Commands.run(this.intakeSubsystem::intake, this.intakeSubsystem)); - driverRightJoystick.button(BUTTON_ID_TRIGGER).whileTrue(Commands.run(() -> { - this.flywheelSubsystem.spinUp((-this.driverRightJoystick.getThrottle() + 1.0) / 2.0); - this.intakeSubsystem.shoot(); - }, this.flywheelSubsystem, this.intakeSubsystem)); + /* + SmartDashboard.putBoolean("outreachMode", true); + SmartDashboard.putNumber("Speed_Limit", limit); + if(!outreachMode){ + driverLeftJoystick.button(BUTTON_ID_TRIGGER) + .whileTrue(Commands.run(this.intakeSubsystem::intake, this.intakeSubsystem)); + driverRightJoystick.button(BUTTON_ID_TRIGGER).whileTrue(Commands.run(() -> { + this.flywheelSubsystem.spinUp((-this.driverRightJoystick.getThrottle() + 1.0) / 2.0); + this.intakeSubsystem.shoot(); + }, this.flywheelSubsystem, this.intakeSubsystem)); + } + */ } - + /* public Command getAutonomousCommand() { return Commands.run(() -> { this.flywheelSubsystem.spinUp(1.0); this.intakeSubsystem.shoot(); }, this.flywheelSubsystem, this.intakeSubsystem).withTimeout(2).andThen(Commands.run(() -> drivetrainSubsystem.arcadeDrive(-0.25, 0, false), drivetrainSubsystem).withTimeout(3.0)); + } + */ } diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index c1b3d3d..3ef0024 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -3,6 +3,7 @@ import static frc.robot.Constants.DrivetrainConstants.DEVICE_ID_LEFT_LEADER; import static frc.robot.Constants.DrivetrainConstants.DEVICE_ID_LEFT_FOLLOWER; import static frc.robot.Constants.DrivetrainConstants.DEVICE_ID_RIGHT_LEADER; +import static frc.robot.Constants.DrivetrainConstants.SPEED_LIMIT; import static frc.robot.Constants.DrivetrainConstants.DEVICE_ID_RIGHT_FOLLOWER; import com.revrobotics.CANSparkMax; @@ -10,18 +11,29 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class DrivetrainSubsystem extends SubsystemBase { private final DifferentialDrive differentialDrive; - + private double newSpeed; + private double limit = 0.4; + private boolean outreachMode = true; + public DrivetrainSubsystem() { CANSparkMax leftLeader = new CANSparkMax(DEVICE_ID_LEFT_LEADER, MotorType.kBrushless); CANSparkMax leftFollower = new CANSparkMax(DEVICE_ID_LEFT_FOLLOWER, MotorType.kBrushless); CANSparkMax rightLeader = new CANSparkMax(DEVICE_ID_RIGHT_LEADER, MotorType.kBrushless); CANSparkMax rightFollower = new CANSparkMax(DEVICE_ID_RIGHT_FOLLOWER, MotorType.kBrushless); + //caution: in future year migrate to elastic or a different software to Smartdashboard because of code crashes in this test version + SmartDashboard.putBoolean("outreachMode", outreachMode); + SmartDashboard.putNumber("Speed_Limit", limit); + leftLeader.restoreFactoryDefaults(); leftFollower.restoreFactoryDefaults(); rightLeader.restoreFactoryDefaults(); @@ -39,16 +51,30 @@ public DrivetrainSubsystem() { leftFollower.follow(leftLeader); rightFollower.follow(rightLeader); - this.differentialDrive = new DifferentialDrive(leftLeader, rightLeader); } public void arcadeDrive(double speed, double rotation) { - this.arcadeDrive(speed, rotation, false); + outreachMode = SmartDashboard.getBoolean("outreachMode", outreachMode); + limit = SmartDashboard.getNumber("Speed_Limit", limit); + if(outreachMode == true){ + newSpeed = speed*limit; + rotation = 0; + } else { + newSpeed = speed*limit; + } + + this.arcadeDrive(newSpeed, rotation, false); } public void arcadeDrive(double speed, double rotation, boolean squareInputs) { - this.differentialDrive.arcadeDrive(speed, rotation, squareInputs); + if(outreachMode == true){ + newSpeed = speed*limit; + rotation = 0; + } else { + newSpeed = speed*limit; + } + this.differentialDrive.arcadeDrive(newSpeed, rotation, squareInputs); } public void tankDrive(double leftSpeed, double rightSpeed) {