diff --git a/src/main/java/frc/robot/Commands/DriveWithJoySticks.java b/src/main/java/frc/robot/Commands/DriveWithJoySticks.java new file mode 100644 index 0000000..e4d579b --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 033c72d..ded601f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 @@ *

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; +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5133735..c6e7fee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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() { + } /** 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..8cad343 --- /dev/null +++ b/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(); + } +} \ No newline at end of file