Justin

src/main/java/frc/robot/Commands/DriveWithJoySticks.java

@@ -0,0 +1,31 @@
+package frc.robot.Commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.Subsystems.Drivetrain;
+
+public class DriveWithJoysticks extends CommandBase{
+    private Drivetrain driveTrain;
+
+    public DriveWithJoysticks(Drivetrain driveTrain){
+        this.driveTrain = driveTrain;
+        addRequirements(driveTrain);
+    }
+    @Override
+    public void initialize(){
+
+    }
+    @Override
+    public void execute(){
+        driveTrain.driveWithJoysticks(RobotContainer.leftJoystick.getRawAxis(Constants.LEFT_JOYSTICK_AXIS),RobotContainer.rightJoystick.getRawAxis(Constants.RIGHT_JOYSTICK_AXIS));
+    }
+    @Override
+    public void end(boolean interrupted){
+        driveTrain.stop();
+    }
+    @Override
+    public boolean isFinished(){
+        return false;
+    }
+}

src/main/java/frc/robot/Constants.java

@@ -1,9 +1,11 @@
 // 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;
 
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+
 /**
  * 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
@@ -12,4 +14,21 @@
  * <p>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 {}
+public final class Constants {
+    //Make global variables all uppercase
+    public static final int LEFT_FRONT_MOTOR_CAN_ID = 0;
+    public static final int RIGHT_FRONT_MOTOR_CAN_ID = 1;
+    public static final int LEFT_REAR_MOTOR_CAN_ID = 2;
+    public static final int RIGHT_REAR_MOTOR_CAN_ID = 3;
+
+    public static final GenericHID.Hand LEFT_JOY_AXIS = Hand.kLeft;
+    public static final GenericHID.Hand RIGHT_JOY_AXIS = Hand.kRight;
+
+    public static final int GYRO_ID = 0;
+    public static final int DISTANCE_SENSORE_PING_CHANNEL = 0;
+    public static final int DISTANCE_SENSORE_ECHO_CHANNEL = 1;
+    public static final int RIGHT_JOYSTICK_CAN_ID = 1;
+    public static final int LEFT_JOYSTICK_CAN_ID = 1;
+    public static final int LEFT_JOYSTICK_AXIS = 1;
+    public static final int RIGHT_JOYSTICK_AXIS = 5;
+}

src/main/java/frc/robot/RobotContainer.java

@@ -6,8 +6,11 @@
 
 import java.util.Set;
 
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Commands.DriveWithJoysticks;
+import frc.robot.Subsystems.Drivetrain;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -22,9 +25,19 @@ public class RobotContainer {
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
+  public static Joystick rightJoystick = new Joystick(Constants.RIGHT_JOYSTICK_CAN_ID);
+  public static Joystick leftJoystick = new Joystick(Constants.LEFT_JOYSTICK_CAN_ID);
+  private Drivetrain driveTrain;
+  private DriveWithJoysticks driveWithJoysticks;
+
   public RobotContainer() {
     // Configure the button bindings
+
+    driveTrain = new Drivetrain();
+    driveWithJoysticks = new DriveWithJoysticks(driveTrain);
+    driveTrain.setDefaultCommand(driveWithJoysticks);
     configureButtonBindings();
+
   }
 
   /**
@@ -34,6 +47,7 @@ public RobotContainer() {
    * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+
   }
 
   /**

src/main/java/frc/robot/Subsystems/Drivetrain.java

@@ -0,0 +1,93 @@
+package frc.robot.Subsystems;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.Ultrasonic;
+import edu.wpi.first.wpilibj.VictorSP;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+
+public class Drivetrain extends SubsystemBase {
+    private VictorSP leftFront, rightFront, leftRear, rightRear;
+    private SpeedControllerGroup leftMotors, rightMotors;
+    private DifferentialDrive drive;
+    private AnalogGyro gyro;
+    private Ultrasonic distanceSensor;
+
+    public Drivetrain(){
+        leftFront = new VictorSP(Constants.LEFT_FRONT_MOTOR_CAN_ID);
+        rightFront = new VictorSP(Constants.RIGHT_REAR_MOTOR_CAN_ID);
+        leftRear = new VictorSP(Constants.LEFT_REAR_MOTOR_CAN_ID);
+        rightRear = new VictorSP(Constants.LEFT_REAR_MOTOR_CAN_ID);
+
+        leftMotors = new SpeedControllerGroup(leftFront,leftRear);
+        rightMotors = new SpeedControllerGroup(rightFront,rightRear);
+        drive = new DifferentialDrive (leftMotors,rightMotors);
+
+        leftMotors.setInverted(true);
+        rightMotors.setInverted(false);
+
+        gyro = new AnalogGyro(Constants.GYRO_ID);
+        distanceSensor = new Ultrasonic(Constants.DISTANCE_SENSORE_PING_CHANNEL, Constants.DISTANCE_SENSORE_ECHO_CHANNEL);
+        Ultrasonic.setAutomaticMode(true);
+
+    }
+    @Override
+    public void periodic() {
+    }
+    //Driving
+    public void driveWithJoysticks(double leftSpeed, double rightSpeed){
+        drive.tankDrive(leftSpeed,rightSpeed);
+    }
+
+    public void stop(){
+        drive.stopMotor();
+    }
+
+    //Gyro Sensor
+    public void initializeGyro(){
+        gyro.initGyro();
+    }
+
+    public double getGyroAngle(){
+        return gyro.getAngle();
+    }
+
+    public double getGyroRate(){
+        return gyro.getRate();
+    }
+
+    public double getGyroCenter(){
+        return gyro.getCenter();
+    }
+
+    public double getGyroOffset(){
+        return gyro.getOffset();
+    }
+    public void stopGyro(){
+        gyro.reset();
+    }
+
+    //Distance Sensor
+    public double getRangeInches(){
+        return distanceSensor.getRangeInches();
+    }
+
+    public void enableDistanceSensor(boolean enable){
+        distanceSensor.setEnabled(enable);
+    }
+
+    public void setDistanceUnits(Ultrasonic.Unit units){
+        distanceSensor.setDistanceUnits(units);
+    }
+
+    public Ultrasonic.Unit getDistanceUnit(){
+        return distanceSensor.getDistanceUnits();
+    }
+
+    public void stopDistanceSensor(){
+        distanceSensor.close();
+    }
+}