diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..e8b60f0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,16 +4,54 @@ 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 + * 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 + *

+ * 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 static class Ports { + public static int kDriveFrontLeftMove = 1; + public static int kDriveFrontLeftTurn = 2; + public static int kDriveFrontRightMove = 3; + public static int kDriveFrontRightTurn = 4; + public static int kDriveBackLeftMove = 5; + public static int kDriveBackLeftTurn = 6; + public static int kDriveBackRightMove = 7; + public static int kDriveBackRightTurn = 8; + } + + public static class MotorConstants{ + public static double movementPerRotation=400; + } + public static class OperatorConstants { public static final int kDriverControllerPort = 0; + public static final int snapButtonNum=1; + } + + public static class PIDConstants { + public static final double kDrivetrainP = 0.15; + public static final double kDrivetrainI = 0.0; + public static final double kDrivetrainD = 0.0; + public static final double kRobotTurningP=0.15; + public static final double kRobotTurningI=0.0; + public static final double kRobotTurningD=0.0; + public static final double snapMotorP = 0.2; + public static final double snapMotorI = 0.2; + public static final double snapMotorD = 0.0; + } + public static class SnapConstants{ + public static final double snapAngleTolerance=1.0; + public static final double snapBearingTolerance=2.0; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..82948aa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,10 +4,17 @@ package frc.robot; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -19,6 +26,10 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + private CANSparkMax motor; + private long startTime; + private long currentTime; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -81,17 +92,29 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + + } @Override public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + + motor = new CANSparkMax(10, MotorType.kBrushless); + startTime=System.currentTimeMillis(); } + /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + currentTime=System.currentTimeMillis()-startTime; + if (currentTime<2000){ + SmartDashboard.putBoolean("running", true); + motor.set(0.1); + } + } /** This function is called once when the robot is first started up. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..8a06cdc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,13 +5,32 @@ package frc.robot; import frc.robot.Constants.OperatorConstants; +// here is where you put all your commands and subsystems; import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; +import frc.robot.commands.Drive; +import frc.robot.commands.Snap; import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Motors; +import frc.robot.Constants.Ports; +import frc.robot.Constants.MotorConstants; +import frc.robot.Constants.OperatorConstants; +import frc.robot.subsystems.Gyroscope; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.I2C; + /** * 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} @@ -21,15 +40,46 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + // the video says to define the motors within the motors subsystem, but I will just define it here so I know where all my + // variables are + + // remember to configure the acutal channels + private final CANSparkMax frontLeftMove= new CANSparkMax(Ports.kDriveFrontLeftMove,MotorType.kBrushless); + private final CANSparkMax frontLeftTurn= new CANSparkMax(Ports.kDriveFrontLeftTurn,MotorType.kBrushless); + private final CANSparkMax frontRightMove= new CANSparkMax(Ports.kDriveFrontRightMove,MotorType.kBrushless); + private final CANSparkMax frontRightTurn= new CANSparkMax(Ports.kDriveFrontRightTurn,MotorType.kBrushless); + private final CANSparkMax backLeftMove= new CANSparkMax(Ports.kDriveBackLeftMove,MotorType.kBrushless); + private final CANSparkMax backLeftTurn = new CANSparkMax(Ports.kDriveBackLeftTurn,MotorType.kBrushless); + private final CANSparkMax backRightMove= new CANSparkMax(Ports.kDriveBackRightMove,MotorType.kBrushless); + private final CANSparkMax backRightTurn= new CANSparkMax(Ports.kDriveBackRightTurn,MotorType.kBrushless); + private final AbsoluteEncoder frontLeftMoveEncoder = frontLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontLeftTurnEncoder = frontLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightMoveEncoder = frontRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightTurnEncoder = frontRightTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftMoveEncoder = backLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftTurnEncoder = backLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightMoveEncoder = backRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightTurnEncoder = backRightTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AHRS gyro = new AHRS(I2C.Port.kMXP); + + + private final Motors motors= new Motors(frontLeftMove, frontLeftTurn, frontRightMove, frontRightTurn, backLeftMove, backLeftTurn, backRightMove, backRightTurn); + //motor radius is configured in mm and distance per rotation is still unkown + private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, MotorConstants.movementPerRotation); + private final Gyroscope gyroscope = new Gyroscope(gyro); + // remember to set the joystick port + + private Joystick stick = new Joystick(OperatorConstants.kDriverControllerPort); + // this is the button on the handle of the joystick + private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButtonNum); + - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); + defaultCommands(); } /** @@ -42,15 +92,12 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); - - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + + } + private void defaultCommands(){ + SmartDashboard.putBoolean("Set default command", false); + motors.setDefaultCommand(new Drive(motors,encoders,gyroscope,stick)); } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java new file mode 100644 index 0000000..3d7ceb5 --- /dev/null +++ b/src/main/java/frc/robot/commands/Drive.java @@ -0,0 +1,183 @@ +package frc.robot.commands; + +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +import frc.robot.subsystems.Gyroscope; +import frc.robot.Constants.PIDConstants; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** An example command that uses an example subsystem. */ +public class Drive extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Encoders encoders; + private final Motors motors; + private final Gyroscope gyro; + private final Joystick joystick; + private PIDController bearingControllerFrontLeft; + private PIDController bearingControllerFrontRight; + private PIDController bearingControllerBackLeft; + private PIDController bearingControllerBackRight; + + public double kP; + public double kI; + public double kD; + + public ShuffleboardTab tab; + // public GenericEntry P; + // public GenericEntry I; + // public GenericEntry D; + + + + + private double currentBearing; + private double previousBearingGoal; + private double fieldOrientOffset; + private double joystickBearing; + + + /** + * Creates a new DriveCommand. + * + * @param motors The motors subsystem used by this command. + * @param encoders The encoders subsystem used by this command. + * @param gyro The gyroscope subsystem used by this command + * @param stick The Joystick used by the driver + */ + public Drive(Motors motors, Encoders encoders, Gyroscope gyro, Joystick stick) { + this.encoders=encoders; + this.motors=motors; + joystick=stick; + this.gyro=gyro; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(motors,encoders,gyro); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + //tuning the PID constants + + // tab= Shuffleboard.getTab("kP"); + // P=tab.add("P constant",0).getEntry(); + kP=PIDConstants.kDrivetrainP; + // tab= Shuffleboard.getTab("kI"); + // I=tab.add("I constant",0).getEntry(); + kI=PIDConstants.kDrivetrainI; + // tab= Shuffleboard.getTab("kD"); + // D=tab.add("D constant",0).getEntry(); + kD=PIDConstants.kDrivetrainD; + + bearingControllerFrontLeft=new PIDController(kP, kI, kD); + bearingControllerFrontLeft.enableContinuousInput(0, Math.PI*2); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontLeft.setTolerance(Math.PI/180); + + bearingControllerFrontRight=new PIDController(kP, kI, kD); + bearingControllerFrontRight.enableContinuousInput(0, Math.PI*2); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(Math.PI/180); + + bearingControllerBackLeft=new PIDController(kP, kI, kD); + bearingControllerBackLeft.enableContinuousInput(0, Math.PI*2); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(Math.PI/180); + + bearingControllerBackRight=new PIDController(kP, kI, kD); + bearingControllerBackRight.enableContinuousInput(0, Math.PI*2); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackRight.setTolerance(Math.PI/180); + motors.setMoveMotors(0); + motors.setTurnMotors(0,TurnMotor.FRONT_LEFT); + motors.setTurnMotors(0,TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(0,TurnMotor.BACK_LEFT); + motors.setTurnMotors(0,TurnMotor.BACK_RIGHT); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); + //Adding Pi to the joystick output so that it is within the range 0 to 2 pi + joystickBearing=joystick.getDirectionRadians(); + if (joystickBearing<0){ + //converting joystick bearing from range 0 to 2pi + joystickBearing+=Math.PI*2; + } + fieldOrientOffset=gyro.getBearing(); + joystickBearing-=fieldOrientOffset; + if (joystickBearing<0){ + //converting the updated joystick bearing from range 0 to 2pi + joystickBearing+=Math.PI*2; + } + SmartDashboard.putNumber("joystick/bearing",joystickBearing); + SmartDashboard.putNumber("gyro offset", fieldOrientOffset); + //I won't update the new bearing unless it is 1 degree different to the old bearing. + if(Math.abs(previousBearingGoal-joystickBearing)>Math.PI/360) + { + //updating the new goal if the joystick is moved + previousBearingGoal=joystickBearing; + bearingControllerFrontLeft.setSetpoint(previousBearingGoal); + bearingControllerFrontLeft.reset(); + bearingControllerFrontRight.setSetpoint(previousBearingGoal); + bearingControllerFrontRight.reset(); + bearingControllerBackLeft.setSetpoint(previousBearingGoal); + bearingControllerBackLeft.reset(); + //due to the tendency for the backright motor to be inversed, I will always set the bearing goal to be + //the current bearing + 180 degrees. + if (previousBearingGoalMath.PI/4){ + // coding the data from 1 to 4 + joystickBearing= joystickBearing/Math.PI*2; + // rounding to closest integer + int estimate= (int) Math.round(joystickBearing); + if (estimate==4){ + //the PIDController doesn't appreciate snapping between 0 and 4, which are essentially the same thing + estimate=0; + } + joystickBearing=estimate; + //adding this because I got a funny feeling about the fact that it is rounded to a long + SmartDashboard.putNumber("approximation", estimate); + //converting back to radians + snapGoal=joystickBearing*Math.PI/2; + //updating the new goal if the joystick is moved + robotBearingController.reset(); + robotBearingController.setSetpoint(snapGoal); + previousSnapGoal=snapGoal; + } + SmartDashboard.putNumber("current Bearing", gyro.getBearing()); + SmartDashboard.putNumber("PID setpoint", robotBearingController.getSetpoint()); + SmartDashboard.putNumber("PID output", robotBearingController.calculate(gyro.getBearing())); + motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())*0.5); + } + } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + SmartDashboard.putBoolean("Command active", false); + motors.setTurnMotors(0, TurnMotor.FRONT_LEFT); + motors.setTurnMotors(0,TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(0, TurnMotor.BACK_LEFT); + motors.setTurnMotors(0, TurnMotor.BACK_RIGHT); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java new file mode 100644 index 0000000..9d4fd95 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -0,0 +1,159 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; + +public class Encoders extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + public AbsoluteEncoder frontLeftMove; + public AbsoluteEncoder frontLeftTurn; + public AbsoluteEncoder frontRightMove; + public AbsoluteEncoder frontRightTurn; + public AbsoluteEncoder backLeftMove; + public AbsoluteEncoder backLeftTurn; + public AbsoluteEncoder backRightMove; + public AbsoluteEncoder backRightTurn; + //for calculating the distance moved + public double frontLeftPosition; + public double frontRightPosition; + public double backLeftPosition; + public double backRightPosition; + public double distanceMoved; + //for calculating the degrees turned + public double distanceRotated; + public double deltaRadians; + //storing value of current bearing of each wheel + public double frontLeftBearing; + public double frontRightBearing; + public double backLeftBearing; + public double backRightBearing; + + /** + * Creates an encoder subsystem to manage the encoders of the robot + * This only works for swerve drives. If you use a different set of encoders for the swerve drive + * you only need to modify the class of the encoder parameters + * + * @param frontLeftMove the encoder of the Front Left Movement Encoder + * @param frontLeftTurn the encoder of the Front Left Turning Encoder + * @param frontRightTurn the encoder of the Front Right Turning Encoder + * @param frontRightMove the encoder of the Front Right Movement Encoder + * @param backLeftMove the encoder of the Back Left Movement Encoder + * @param backLeftTurn the encoder of the Back Left Turning Encoder + * @param backRightMove the encoder of he Back Right Movement Encoder + * @param backRightTurn the encoder of the Back Right Turning Encoder + * @param movementPerRotation the distance that the robot moves for every rotation of the Movement Encoder + */ + public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, AbsoluteEncoder frontRightTurn, AbsoluteEncoder frontRightMove, AbsoluteEncoder backLeftMove, AbsoluteEncoder backLeftTurn, AbsoluteEncoder backRightMove, AbsoluteEncoder backRightTurn, double movementPerRotation) { + this.frontLeftMove = frontLeftMove; + this.frontLeftTurn = frontLeftTurn; + this.frontRightTurn = frontRightTurn; + this.frontRightMove = frontRightMove; + this.backLeftMove = backLeftMove; + this.backLeftTurn = backLeftTurn; + this.backRightMove = backRightMove; + this.backRightTurn = backRightTurn; + + //the movement motors use movement per rotation because 1 rotation for the moveement motor doesn't mean that + //the robot moves forward by precisely the circumference of the motor + frontLeftMove.setPositionConversionFactor(movementPerRotation); + frontRightMove.setPositionConversionFactor(movementPerRotation); + backLeftMove.setPositionConversionFactor(movementPerRotation); + backRightMove.setPositionConversionFactor(movementPerRotation); + frontLeftBearing = 0; + frontRightBearing = 0; + backLeftBearing = 0; + backRightBearing = 0; + frontLeftPosition=0; + frontRightPosition=0; + backLeftPosition=0; + backRightPosition=0; + } + + /** + * This gets the distanceMoved by each since the last time this function was called + * @param motorNum the number of the motor you want to check + * @return the distance Moved since the last time this function was called + */ + public double getDistanceMoved(int motorNum){ + switch (motorNum){ + case 1: + //return frontLeft distance + distanceMoved= frontLeftMove.getPosition()-frontLeftPosition; + frontLeftPosition=frontLeftMove.getPosition(); + return distanceMoved; + case 2: + //return frontRight distance + distanceMoved= frontRightMove.getPosition()-frontRightPosition; + frontRightPosition=frontRightMove.getPosition(); + return distanceMoved; + case 3: + //return backLeft distance + distanceMoved= backLeftMove.getPosition()-backLeftPosition; + backLeftPosition=backLeftMove.getPosition(); + return distanceMoved; + case 4: + //return backRight distance + distanceMoved= backRightMove.getPosition()-backRightPosition; + backRightPosition=backRightMove.getPosition(); + return distanceMoved; + } + //this should never happen, but just in case + return 0.0; + } + + public enum TurnEncoder{ + FRONT_LEFT,FRONT_RIGHT, BACK_LEFT, BACK_RIGHT + } + + /** + * This returns the currrent bearing of any one of the Turn Encoders in radians + * The range is from 0 to 2 PI radians + * @param encoder the TurnEncoder you would like to check + * @return the bearing of the encoder from 0 to 2PI Radians + */ + public double motorTurned(TurnEncoder encoder){ + switch (encoder){ + case FRONT_LEFT: + //return frontLeft degrees turned + distanceRotated = frontLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); + //using radians + frontLeftBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down + frontLeftBearing = frontLeftBearing%(Math.PI*2); + return frontLeftBearing; + case FRONT_RIGHT: + //return frontRight degrees turned + distanceRotated = frontRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); + //using radians + frontRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down + frontRightBearing = frontRightBearing%(Math.PI*2); + return frontRightBearing; + case BACK_LEFT: + //return backLeft degrees turned + distanceRotated = backLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); + //using radians + backLeftBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down + backLeftBearing = backLeftBearing%(Math.PI*2); + return backLeftBearing; + case BACK_RIGHT: + //return backRight degrees turned + distanceRotated = backRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); + //using radians + backRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down + backRightBearing = backRightBearing%(Math.PI*2); + return backRightBearing; + } + //this should never happen, but just so the code is happy + return 0.0; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java new file mode 100644 index 0000000..141b100 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.kauailabs.navx.frc.AHRS; + + + +public class Gyroscope extends SubsystemBase { + public AHRS gyro; + public double convertedBearing; + /** + * This creates the Gyroscope subsystem to manage the bearing of the robot + * @param Gyro the AHRS object representing the gyro + */ + public Gyroscope(AHRS Gyro){ + gyro=Gyro; + } + /** + * This returns the bearing of the robot + * @return Returns the bearing of the robot from range 0 to 2PI radians. + */ + public double getBearing(){ + //returns bearing in radians + convertedBearing= gyro.getAngle()%360; + if (convertedBearing<0){ + convertedBearing+=360; + } + convertedBearing=convertedBearing/180*Math.PI; + return convertedBearing; + } +} + diff --git a/src/main/java/frc/robot/subsystems/Motors.java b/src/main/java/frc/robot/subsystems/Motors.java new file mode 100644 index 0000000..b8fc2d2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; + + + + +public class Motors extends SubsystemBase { + public static CANSparkMax frontLeftMove; + public static CANSparkMax frontLeftTurn; + public static CANSparkMax frontRightMove; + public static CANSparkMax frontRightTurn; + public static CANSparkMax backLeftMove; + public static CANSparkMax backLeftTurn; + public static CANSparkMax backRightMove; + public static CANSparkMax backRightTurn; + + /** + * This creates the Motors subsystem to manage what power the motors are set at. + * This only works for a swerve drive. + * @param FrontLeftMove the Front Left Movement Motor + * @param FrontLeftTurn the Front Left Turning Motor + * @param FrontRightMove the Front Right Movement Motor + * @param FrontRightTurn the Front Right Turning Motor + * @param BackLeftMove the Back Left Movement Motor + * @param BackLeftTurn the Back Left Turning Motor + * @param BackRightMove the Back Right Movement Motor + * @param BackRightTurn the Back Right Turning Motor + */ + public Motors(CANSparkMax FrontLeftMove, CANSparkMax FrontLeftTurn, CANSparkMax FrontRightMove, CANSparkMax FrontRightTurn, CANSparkMax BackLeftMove, CANSparkMax BackLeftTurn, CANSparkMax BackRightMove, CANSparkMax BackRightTurn) { + frontLeftMove=FrontLeftMove; + frontLeftTurn=FrontLeftTurn; + frontRightMove=FrontRightMove; + frontRightTurn=FrontRightTurn; + backLeftMove=BackLeftMove; + backLeftTurn=BackLeftTurn; + backRightMove=BackRightMove; + backRightTurn=BackRightTurn; + + //setting all move motors to follow the Front Left motors + frontRightMove.follow(FrontLeftMove); + backLeftMove.follow(FrontLeftMove); + } + /** + * This sets the move motors to go at a certain power + * @param PIDoutput The power that you want to set the motors at, ideally controller by a + * PID controller. Only takes from range -1 to 1. + */ + public void setMoveMotors(double PIDoutput){ + frontLeftMove.set(PIDoutput); + //backRightMotor is inverted + backRightMove.set(PIDoutput); + } + + public enum TurnMotor { + FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT + } + /** + * This sets the turn motors to go at a certain power + * @param PIDOutput The power that you want to set the motors at, ideally controller by a + * PID controller. Only takes from range -1 to 1. + * @param motor The Turn motor which you would like to set. + */ + public void setTurnMotors(double PIDOutput, TurnMotor motor){ + switch (motor) + { + case FRONT_LEFT: + frontLeftTurn.set(PIDOutput); + case FRONT_RIGHT: + frontRightTurn.set(PIDOutput); + case BACK_LEFT: + backLeftTurn.set(PIDOutput); + case BACK_RIGHT: + backRightTurn.set(PIDOutput); + } + } +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..e978a5f --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file