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 distancePerRotation=100; + public static double movementPerRotation=400; + } + public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + + public static class PIDConstants { + public static final double kDrivetrainP = 0.2; + public static final double kDrivetrainI = 0.0; + public static final double kDrivetrainD = 0.0; + public static final double kRobotTurnP = 0.2; + public static final double kRobotTurnI = 0.0; + public static final double kRobotTurnD = 0.0; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..a0fc394 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,9 @@ 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.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -18,6 +20,11 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private Talon FrontLeftMove; + private Talon FrontRightMove; + private Talon BackLeftMove; + private Talon BackRightMove; + private Encoder frontLeftMove; /** * This function is run when the robot is first started up and should be used for any @@ -87,11 +94,28 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + FrontLeftMove= new Talon(0); + FrontRightMove= new Talon(0); + BackLeftMove= new Talon(0); + BackRightMove= new Talon(0); + frontLeftMove = new Encoder(0, 0); + FrontLeftMove.set(0.1); + FrontRightMove.set(0.1); + BackLeftMove.set(0.1); + BackRightMove.set(0.1); } + /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + if (frontLeftMove.get()==100){ + FrontLeftMove.set(0); + FrontRightMove.set(0); + BackLeftMove.set(0); + BackRightMove.set(0); + } + } /** 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..8090588 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,27 @@ 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.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.CANSparkMax; +import com.revrobotics.RelativeEncoder; +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 @@ -21,15 +35,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 RelativeEncoder frontLeftMoveEncoder = frontLeftMove.getEncoder(); + private final RelativeEncoder frontLeftTurnEncoder = frontLeftTurn.getEncoder(); + private final RelativeEncoder frontRightMoveEncoder = frontRightMove.getEncoder(); + private final RelativeEncoder frontRightTurnEncoder = frontRightTurn.getEncoder(); + private final RelativeEncoder backLeftMoveEncoder = backLeftMove.getEncoder(); + private final RelativeEncoder backLeftTurnEncoder = backLeftTurn.getEncoder(); + private final RelativeEncoder backRightMoveEncoder = backRightMove.getEncoder(); + private final RelativeEncoder backRightTurnEncoder = backRightTurn.getEncoder(); + 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, 38.1, MotorConstants.distancePerRotation,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, 1); + - // 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 +87,14 @@ 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()); - } + snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick)); + + } + private void defaultCommands(){ + 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/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/AutoTurnDegrees.java new file mode 100644 index 0000000..6f2096f --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoTurnDegrees.java @@ -0,0 +1,124 @@ +// 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 frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.Gyroscope; +import frc.robot.Constants.PIDConstants; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class AutoTurnDegrees extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Motors motors; + private final Encoders encoders; + private final Gyroscope gyro; + private double angleGoal; + + private double kP; + private double kI; + private double kD; + private PIDController frontLeftBearingController; + private PIDController frontRightBearingController; + private PIDController backLeftBearingController; + private PIDController backRightBearingController; + + private double kPRobotTurn; + private double kIRobotTurn; + private double kDRobotTurn; + private PIDController robotBearingController; + private double currentBearing; + + private boolean endCommand; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AutoTurnDegrees(Motors motors, Encoders encoders, Gyroscope gyro, double angleGoal) { + this.motors=motors; + this.encoders=encoders; + this.gyro=gyro; + this.angleGoal=angleGoal; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(motors,encoders,gyro); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + kP=PIDConstants.kDrivetrainP; + kI=PIDConstants.kDrivetrainI; + kD=PIDConstants.kDrivetrainD; + + kPRobotTurn=PIDConstants.kRobotTurnP; + kIRobotTurn=PIDConstants.kRobotTurnI; + kDRobotTurn=PIDConstants.kRobotTurnD; + + frontLeftBearingController= new PIDController(kP, kI, kD); + frontLeftBearingController.enableContinuousInput(0, Math.PI*2); + frontLeftBearingController.setSetpoint(Math.PI/4); + + frontRightBearingController= new PIDController(kP, kI, kD); + frontRightBearingController.enableContinuousInput(0, Math.PI*2); + frontRightBearingController.setSetpoint(Math.PI*3/4); + + backLeftBearingController= new PIDController(kP, kI, kD); + backLeftBearingController.enableContinuousInput(0, Math.PI*2); + backLeftBearingController.setSetpoint(Math.PI*5/4); + + backRightBearingController= new PIDController(kP, kI, kD); + backRightBearingController.enableContinuousInput(0, Math.PI*2); + backRightBearingController.setSetpoint(Math.PI*7/4); + + robotBearingController= new PIDController(kPRobotTurn, kIRobotTurn, kDRobotTurn); + robotBearingController.enableContinuousInput(0, Math.PI*2); + robotBearingController.setSetpoint(angleGoal); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (frontLeftBearingController.atSetpoint()&& + frontRightBearingController.atSetpoint()&& + backLeftBearingController.atSetpoint()&& + backRightBearingController.atSetpoint()) + { + motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())); + if(robotBearingController.atSetpoint()){ + endCommand=true; + } + } + else + { + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + motors.setTurnMotors(frontLeftBearingController.calculate(currentBearing), TurnMotor.FRONT_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + motors.setTurnMotors(frontRightBearingController.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + motors.setTurnMotors(backLeftBearingController.calculate(currentBearing), TurnMotor.BACK_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + motors.setTurnMotors(backRightBearingController.calculate(currentBearing), TurnMotor.BACK_RIGHT); + } + } + + // 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 endCommand; + } +} 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..c99258c --- /dev/null +++ b/src/main/java/frc/robot/commands/Drive.java @@ -0,0 +1,141 @@ +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; +/** 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; + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + 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); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + fieldOrientOffset=gyro.getBearing(); + if(previousBearingGoal!=joystick.getDirectionRadians()) + { + //updating the new goal if the joystick is moved + previousBearingGoal=joystick.getDirectionRadians(); + bearingControllerFrontLeft.setSetpoint(previousBearingGoal); + bearingControllerFrontLeft.reset(); + bearingControllerFrontRight.setSetpoint(previousBearingGoal); + bearingControllerFrontRight.reset(); + bearingControllerBackLeft.setSetpoint(previousBearingGoal); + bearingControllerBackLeft.reset(); + bearingControllerBackRight.setSetpoint(previousBearingGoal); + bearingControllerBackRight.reset(); + } + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); + //setting the speed, but at 0.5 scale to ensure no one dies + motors.setMoveMotors(joystick.getMagnitude()*0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + 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); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} + diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java new file mode 100644 index 0000000..4c827c6 --- /dev/null +++ b/src/main/java/frc/robot/commands/Snap.java @@ -0,0 +1,169 @@ +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 edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class Snap extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Motors motors; + private final Encoders encoders; + private final Gyroscope gyro; + private final Joystick stick; + + private PIDController bearingControllerFrontLeft; + private PIDController bearingControllerFrontRight; + private PIDController bearingControllerBackLeft; + private PIDController bearingControllerBackRight; + private PIDController robotBearingController; + + public ShuffleboardTab tab; + public GenericEntry P; + public GenericEntry I; + public GenericEntry D; + //for the motors + public double kP; + public double kI; + public double kD; + //for the robot + public double kPRobot; + public double kIRobot; + public double kDRobot; + + private double joystickBearing; + private double currentBearing; + private double snapGoal; + private double previousSnapGoal; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public Snap(Motors motors, Encoders encoders, Gyroscope gyro, Joystick stick) { + this.motors = motors; + this.encoders= encoders; + this.gyro=gyro; + this.stick= stick; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(motors, encoders, gyro); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + tab= Shuffleboard.getTab("kPMotor"); + P=tab.add("Motor P constant",0).getEntry(); + kP=P.getDouble(0); + tab= Shuffleboard.getTab("kIMotor"); + I=tab.add("Motor I constant",0).getEntry(); + kI=I.getDouble(0); + tab= Shuffleboard.getTab("kDMotor"); + D=tab.add("Motor D constant",0).getEntry(); + kD=D.getDouble(0); + + tab= Shuffleboard.getTab("kPRobot"); + P=tab.add("Robot P constant",0).getEntry(); + kPRobot=P.getDouble(0); + tab= Shuffleboard.getTab("kIRobot"); + I=tab.add("Robot I constant",0).getEntry(); + kIRobot=I.getDouble(0); + tab= Shuffleboard.getTab("kDRobot"); + D=tab.add("Robot D constant",0).getEntry(); + kDRobot=D.getDouble(0); + + // first the turn motors need to get ready for turning + bearingControllerFrontLeft=new PIDController(kP, kI, kD); + bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontLeft.setTolerance(Math.PI/360); + bearingControllerFrontLeft.setSetpoint(Math.PI/4); + + bearingControllerFrontRight=new PIDController(kP, kI, kD); + bearingControllerFrontRight.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(Math.PI/360); + bearingControllerFrontRight.setSetpoint(Math.PI*3/4); + + bearingControllerBackLeft=new PIDController(kP, kI, kD); + bearingControllerBackLeft.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(Math.PI/360); + bearingControllerBackLeft.setSetpoint(Math.PI*5/4); + + bearingControllerBackRight=new PIDController(kP, kI, kD); + bearingControllerBackRight.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackRight.setTolerance(Math.PI/360); + bearingControllerBackRight.setSetpoint(Math.PI*7/4); + + //the PID controller for the robot itself after turning mode has been achieved + robotBearingController= new PIDController(kPRobot, kIRobot, kDRobot); + robotBearingController.enableContinuousInput(0, 2*Math.PI); + robotBearingController.setTolerance(Math.PI/360); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //if any of the motors are not ready to turn, this code won't run + if ( + bearingControllerFrontLeft.atSetpoint()==false || + bearingControllerFrontRight.atSetpoint()==false || + bearingControllerBackLeft.atSetpoint()==false || + bearingControllerBackRight.atSetpoint()==false){ + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); + } + else{ + //robot ready to rotate + joystickBearing=stick.getDirectionRadians(); + // coding the data from 1 to 4 + joystickBearing= joystickBearing/Math.PI*2; + // rounding to closest integer + int estimate=(int) joystickBearing; + joystickBearing=estimate; + //converting back to radians + snapGoal=joystickBearing*Math.PI/2; + if (snapGoal!=previousSnapGoal){ + //updating the new goal if the joystick is moved + robotBearingController.reset(); + robotBearingController.setSetpoint(snapGoal); + previousSnapGoal=snapGoal; + } + motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())); + } + } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + 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..574382a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -0,0 +1,148 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.RelativeEncoder; + +public class Encoders extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + public RelativeEncoder frontLeftMove; + public RelativeEncoder frontLeftTurn; + public RelativeEncoder frontRightMove; + public RelativeEncoder frontRightTurn; + public RelativeEncoder backLeftMove; + public RelativeEncoder backLeftTurn; + public RelativeEncoder backRightMove; + public RelativeEncoder 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 motorRadius; + 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; + + public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, RelativeEncoder frontRightTurn, RelativeEncoder frontRightMove, RelativeEncoder backLeftMove, RelativeEncoder backLeftTurn, RelativeEncoder backRightMove, RelativeEncoder backRightTurn, double motorRadius, double distancePerRotation, 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; + + + // Reset encoder positions + frontLeftMove.setPosition(0); + frontRightMove.setPosition(0); + backLeftMove.setPosition(0); + backRightMove.setPosition(0); + frontLeftTurn.setPosition(0); + frontRightTurn.setPosition(0); + backLeftTurn.setPosition(0); + backRightTurn.setPosition(0); + + //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); + frontLeftTurn.setPositionConversionFactor(distancePerRotation); + frontRightTurn.setPositionConversionFactor(distancePerRotation); + backLeftTurn.setPositionConversionFactor(distancePerRotation); + backRightTurn.setPositionConversionFactor(distancePerRotation); + this.motorRadius = motorRadius; + frontLeftBearing = 0; + frontRightBearing = 0; + backLeftBearing = 0; + backRightBearing = 0; + frontLeftPosition=0; + frontRightPosition=0; + backLeftPosition=0; + backRightPosition=0; + } + 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 + } + public double motorTurned(TurnEncoder encoder){ + switch (encoder){ + case FRONT_LEFT: + //return frontLeft degrees turned + distanceRotated = frontLeftTurn.getPosition(); + //using radians + deltaRadians = distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + frontLeftBearing += deltaRadians; + frontLeftBearing = frontLeftBearing%(Math.PI*2); + return frontLeftBearing; + case FRONT_RIGHT: + //return frontLeft degrees turned + distanceRotated = frontLeftTurn.getPosition(); + //using radians + deltaRadians = distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + frontRightBearing += deltaRadians; + frontRightBearing = frontRightBearing%(Math.PI*2); + return frontRightBearing; + case BACK_LEFT: + //return frontLeft degrees turned + distanceRotated = frontLeftTurn.getPosition(); + //using radians + deltaRadians = distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + backLeftBearing += deltaRadians; + backLeftBearing = backLeftBearing%(Math.PI*2); + return backLeftBearing; + case BACK_RIGHT: + //return frontLeft degrees turned + distanceRotated = frontLeftTurn.getPosition(); + //using radians + deltaRadians = distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + backRightBearing += deltaRadians; + backRightBearing = backRightBearing%(Math.PI*2); + return backRightBearing; + } + //this should never happen, but just so the code is happy + return 0.0; + } +} + + + 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..1841aa8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -0,0 +1,22 @@ +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; + public Gyroscope(AHRS Gyro){ + gyro=Gyro; + } + public double getBearing(){ + //returns bearing in radians + convertedBearing= gyro.getAngle()%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..0c02915 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -0,0 +1,57 @@ +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; + + + 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); + backRightMove.follow(FrontLeftMove); + } + public void setMoveMotors(double PIDoutput){ + frontLeftMove.set(PIDoutput); + } + + public enum TurnMotor { + FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT + } + + 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