diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 033c72d..0000000 --- a/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants {} diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java new file mode 100644 index 0000000..8362d34 --- /dev/null +++ b/src/main/java/frc/robot/OI.java @@ -0,0 +1,8 @@ +// Operator Interface +package frc.robot; + +import edu.wpi.first.wpilibj.XboxController; + +public class OI { + public XboxController driverController = new XboxController(RobotMap.CONTROLLER_PORT); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 99b0c8d..b31bfdc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,16 +7,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import com.kauailabs.navx.frc.AHRS; - - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -25,10 +15,6 @@ * project. */ public class Robot extends TimedRobot { - - AHRS ahrs; - Joystick stick; - private Command m_autonomousCommand; private RobotContainer m_robotContainer; @@ -39,15 +25,6 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - stick = new Joystick(0); - try { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - ahrs = new AHRS(SPI.Port.kMXP); - } catch (RuntimeException ex ) { - DriverStation.reportError("Error instantiating navX-MXP: " + ex.getMessage(), true); - } // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -104,95 +81,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - while (isEnabled()) { - - Timer.delay(0.020); /* wait for one motor update time period (50Hz) */ - - boolean zero_yaw_pressed = stick.getTrigger(); - if ( zero_yaw_pressed ) { - ahrs.zeroYaw(); - } - - /* Display 6-axis Processed Angle Data */ - SmartDashboard.putBoolean( "IMU_Connected", ahrs.isConnected()); - SmartDashboard.putBoolean( "IMU_IsCalibrating", ahrs.isCalibrating()); - SmartDashboard.putNumber( "IMU_Yaw", ahrs.getYaw()); - SmartDashboard.putNumber( "IMU_Pitch", ahrs.getPitch()); - SmartDashboard.putNumber( "IMU_Roll", ahrs.getRoll()); - - /* Display tilt-corrected, Magnetometer-based heading (requires */ - /* magnetometer calibration to be useful) */ - - SmartDashboard.putNumber( "IMU_CompassHeading", ahrs.getCompassHeading()); - - /* Display 9-axis Heading (requires magnetometer calibration to be useful) */ - SmartDashboard.putNumber( "IMU_FusedHeading", ahrs.getFusedHeading()); - - /* These functions are compatible w/the WPI Gyro Class, providing a simple */ - /* path for upgrading from the Kit-of-Parts gyro to the navx-MXP */ - - SmartDashboard.putNumber( "IMU_TotalYaw", ahrs.getAngle()); - SmartDashboard.putNumber( "IMU_YawRateDPS", ahrs.getRate()); - - /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) */ - - SmartDashboard.putNumber( "IMU_Accel_X", ahrs.getWorldLinearAccelX()); - SmartDashboard.putNumber( "IMU_Accel_Y", ahrs.getWorldLinearAccelY()); - SmartDashboard.putBoolean( "IMU_IsMoving", ahrs.isMoving()); - SmartDashboard.putBoolean( "IMU_IsRotating", ahrs.isRotating()); - - /* Display estimates of velocity/displacement. Note that these values are */ - /* not expected to be accurate enough for estimating robot position on a */ - /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding */ - /* of these errors due to single (velocity) integration and especially */ - /* double (displacement) integration. */ - - SmartDashboard.putNumber( "Velocity_X", ahrs.getVelocityX()); - SmartDashboard.putNumber( "Velocity_Y", ahrs.getVelocityY()); - SmartDashboard.putNumber( "Displacement_X", ahrs.getDisplacementX()); - SmartDashboard.putNumber( "Displacement_Y", ahrs.getDisplacementY()); - - /* Display Raw Gyro/Accelerometer/Magnetometer Values */ - /* NOTE: These values are not normally necessary, but are made available */ - /* for advanced users. Before using this data, please consider whether */ - /* the processed data (see above) will suit your needs. */ - - SmartDashboard.putNumber( "RawGyro_X", ahrs.getRawGyroX()); - SmartDashboard.putNumber( "RawGyro_Y", ahrs.getRawGyroY()); - SmartDashboard.putNumber( "RawGyro_Z", ahrs.getRawGyroZ()); - SmartDashboard.putNumber( "RawAccel_X", ahrs.getRawAccelX()); - SmartDashboard.putNumber( "RawAccel_Y", ahrs.getRawAccelY()); - SmartDashboard.putNumber( "RawAccel_Z", ahrs.getRawAccelZ()); - SmartDashboard.putNumber( "RawMag_X", ahrs.getRawMagX()); - SmartDashboard.putNumber( "RawMag_Y", ahrs.getRawMagY()); - SmartDashboard.putNumber( "RawMag_Z", ahrs.getRawMagZ()); - SmartDashboard.putNumber( "IMU_Temp_C", ahrs.getTempC()); - - /* Omnimount Yaw Axis Information */ - /* For more info, see http://navx-mxp.kauailabs.com/installation/omnimount */ - AHRS.BoardYawAxis yaw_axis = ahrs.getBoardYawAxis(); - SmartDashboard.putString( "YawAxisDirection", yaw_axis.up ? "Up" : "Down" ); - SmartDashboard.putNumber( "YawAxis", yaw_axis.board_axis.getValue() ); - - /* Sensor Board Information */ - SmartDashboard.putString( "FirmwareVersion", ahrs.getFirmwareVersion()); - - /* Quaternion Data */ - /* Quaternions are fascinating, and are the most compact representation of */ - /* orientation data. All of the Yaw, Pitch and Roll Values can be derived */ - /* from the Quaternions. If interested in motion processing, knowledge of */ - /* Quaternions is highly recommended. */ - SmartDashboard.putNumber( "QuaternionW", ahrs.getQuaternionW()); - SmartDashboard.putNumber( "QuaternionX", ahrs.getQuaternionX()); - SmartDashboard.putNumber( "QuaternionY", ahrs.getQuaternionY()); - SmartDashboard.putNumber( "QuaternionZ", ahrs.getQuaternionZ()); - - /* Connectivity Debugging Support */ - SmartDashboard.putNumber( "IMU_Byte_Count", ahrs.getByteCount()); - SmartDashboard.putNumber( "IMU_Update_Count", ahrs.getUpdateCount()); - } - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f18779d..6f41896 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,35 +6,84 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +// import edu.wpi.first.wpilibj.GenericHID.Hand; +// import frc.robot.commands.StartPneumatics; +import frc.robot.commands.StartDriving; +// import frc.robot.subsystems.Pneumatics; +import frc.robot.subsystems.Drivetrain; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; /** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * This class is where the bulk of the robot should be declared. Since + * Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in + * the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of + * the robot (including * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + public static Drivetrain drivetrain; + // public static Pneumatics pneumatics; - private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); + public static XboxController xbox; + public static JoystickButton solenoidButton; + public static JoystickButton solenoidOffButton; - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { - // Configure the button bindings + drivetrain = new Drivetrain(); + // pneumatics = new Pneumatics(); configureButtonBindings(); + + drivetrain.setDefaultCommand(new StartDriving()); + // pneumatics.setDefaultCommand(new StartPneumatics()); + } + + public static double getYLeft(){ + double kleft = xbox.getLeftY(); + if(Math.abs(kleft) <= 0.1){ + return 0; + } else { + return -kleft*Math.abs(kleft); //Math.abs to preserve sign + } + } + + public static double getYRight(){ + double kright = xbox.getRightY(); + if(Math.abs(kright) <= 0.1){ + return 0; + } else { + return kright*Math.abs(kright); //Math.abs to preserve sign + } + } + + public static boolean getSolenoidButton() { + return solenoidButton.getAsBoolean(); + } + + public static boolean getSolenoidOffButton() { + return solenoidButton.getAsBoolean(); } /** - * Use this method to define your button->command mappings. Buttons can be created by + * Use this method to define your button->command mappings. Buttons can be + * created by * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing + * it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureButtonBindings() {} + + private void configureButtonBindings() { + xbox = new XboxController(0); + solenoidButton = new JoystickButton(xbox, 0); + solenoidOffButton = new JoystickButton(xbox, 1); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -43,6 +92,6 @@ private void configureButtonBindings() {} */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return m_autoCommand; + return new StartDriving(); } } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java new file mode 100644 index 0000000..81b4ce6 --- /dev/null +++ b/src/main/java/frc/robot/RobotMap.java @@ -0,0 +1,16 @@ +package frc.robot; +//PORT DECLARATIONS + +public class RobotMap { + + // CONTROLLER + public static final int CONTROLLER_PORT = 0; + + // DRIVETRAIN MOTORS + public static final int right = 5; + public static final int left = 2; + + public static final int rightSlave = 6; + public static final int leftSlave = 3; + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/StartDriving.java similarity index 53% rename from src/main/java/frc/robot/commands/ExampleCommand.java rename to src/main/java/frc/robot/commands/StartDriving.java index abd6a0e..2837c3c 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/commands/StartDriving.java @@ -4,23 +4,13 @@ package frc.robot.commands; -import frc.robot.subsystems.ExampleSubsystem; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); +public class StartDriving extends CommandBase { + /** Creates a new StartDriving. */ + public StartDriving() { + addRequirements(RobotContainer.drivetrain); } // Called when the command is initially scheduled. @@ -29,7 +19,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + RobotContainer.drivetrain.tankDrive(RobotContainer.getYLeft(), RobotContainer.getYRight()); + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/StartPneumatics.java b/src/main/java/frc/robot/commands/StartPneumatics.java new file mode 100644 index 0000000..ae6dbe0 --- /dev/null +++ b/src/main/java/frc/robot/commands/StartPneumatics.java @@ -0,0 +1,40 @@ +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.commands; + +// import edu.wpi.first.wpilibj2.command.CommandBase; +// import frc.robot.Robot; +// import frc.robot.RobotContainer; + +// public class StartPneumatics extends CommandBase { +// /** Creates a new StartPneumatics. */ +// public StartPneumatics() { +// addRequirements(RobotContainer.pneumatics); +// } + +// // 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() { +// if (RobotContainer.getSolenoidOffButton()) { +// RobotContainer.pneumatics.solenoidOff(); +// } else if(RobotContainer.getSolenoidButton()) { +// RobotContainer.pneumatics.toggleSolenoid(); +// } +// } + +// // 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/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..9414ce1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import frc.robot.RobotMap; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +public class Drivetrain extends SubsystemBase { + + CANSparkMax right, left; + CANSparkMax rightSlave, leftSlave; + + public Drivetrain() { + right = new CANSparkMax(RobotMap.right, MotorType.kBrushless); + left = new CANSparkMax(RobotMap.left, MotorType.kBrushless); + rightSlave = new CANSparkMax(RobotMap.rightSlave, MotorType.kBrushless); + leftSlave = new CANSparkMax(RobotMap.leftSlave, MotorType.kBrushless); + + //set slaves + rightSlave.follow(right); + leftSlave.follow(left); + stop(); + } + + + public void tankDrive(double left, double right){ + this.left.set(left); + this.right.set(right); + } + + public void stop(){ + left.stopMotor(); + right.stopMotor(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index f5e9470..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/Pneumatics.java b/src/main/java/frc/robot/subsystems/Pneumatics.java new file mode 100644 index 0000000..431cf33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Pneumatics.java @@ -0,0 +1,47 @@ +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.subsystems; + +// import edu.wpi.first.wpilibj.Compressor; +// import edu.wpi.first.wpilibj.DoubleSolenoid; +// import edu.wpi.first.wpilibj.PneumaticsModuleType; +// import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +// import edu.wpi.first.wpilibj.Compressor; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// public class Pneumatics extends SubsystemBase { + +// Compressor pcmCompressor; +// DoubleSolenoid doublePCM; + +// /** Creates a new Chassis. */ +// public Pneumatics() { +// // Compressor pcmCompressor = new Compressor(0, PneumaticsModuleType.CTREPCM); + +// // pcmCompressor.enableDigital(); +// // pcmCompressor.disable(); + +// // boolean enabled = pcmCompressor.isEnabled(); +// // boolean pressureSwitch = pcmCompressor.getPressureSwitchValue(); +// // double current = pcmCompressor.getCurrent(); + +// // doublePCM = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 0, 1); +// // doublePCM.set(Value.kReverse); +// pcmCompressor = new Compressor(0, PneumaticsModuleType.CTREPCM); +// } + +// @Override +// public void periodic() { +// // This method will be called once per scheduler run +// } + +// public void toggleSolenoid() { +// pcmCompressor.enableDigital(); +// } + +// public void solenoidOff() { +// pcmCompressor.disable(); +// } +// }