Feb 10 initial

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);
+}

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() {

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();
   }
 }

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;
+
+}

src/main/java/frc/robot/commands/ExampleCommand.java → 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

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;
+//   }
+// }