From 9147e1afd7eb0ef7d6339087d45b51db62ffb08d Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sun, 28 Jan 2024 16:50:07 +0000 Subject: [PATCH 01/19] Created the basic code for the driver controlled swerve drive Some values need updating: Motor channels Encoder channels DistancePerPulse --- src/main/java/frc/robot/RobotContainer.java | 39 +++++- src/main/java/frc/robot/commands/Drive.java | 111 ++++++++++++++++++ .../java/frc/robot/subsystems/Encoders.java | 111 ++++++++++++++++++ .../java/frc/robot/subsystems/Gyroscope.java | 19 +++ .../java/frc/robot/subsystems/Motors.java | 48 ++++++++ 5 files changed, 327 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/Drive.java create mode 100644 src/main/java/frc/robot/subsystems/Encoders.java create mode 100644 src/main/java/frc/robot/subsystems/Gyroscope.java create mode 100644 src/main/java/frc/robot/subsystems/Motors.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..014a9d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,12 +5,19 @@ 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.subsystems.ExampleSubsystem; +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Motors; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.Encoder; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -21,6 +28,34 @@ 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 Talon FrontLeftMove= new Talon(0); + private final Talon FrontLeftTurn= new Talon(2); + private final Talon FrontRightMove= new Talon(3); + private final Talon FrontRightTurn= new Talon(4); + private final Talon BackLeftMove= new Talon(5); + private final Talon BackLeftTurn = new Talon(6); + private final Talon BackRightMove= new Talon(7); + private final Talon BackRightTurn= new Talon(8); + private final Encoder frontLeftMove = new Encoder(0, 0); + private final Encoder frontLeftTurn = new Encoder(0,0); + private final Encoder frontRightMove = new Encoder(0, 0); + private final Encoder frontRightTurn = new Encoder(0,0); + private final Encoder backLeftMove = new Encoder(0, 0); + private final Encoder backLeftTurn = new Encoder(0,0); + private final Encoder backRightMove = new Encoder(0, 0); + private final Encoder backRightTurn = new Encoder(0,0); + + + private final Motors motors= new Motors(FrontLeftMove, FrontLeftTurn, FrontRightMove, FrontRightTurn, BackLeftMove, BackLeftTurn, BackRightMove, BackRightTurn); + //motor radius is configured in mm and distance per pulse is still unkown + private final Encoders encoders= new Encoders(frontLeftMove, frontLeftTurn, frontRightTurn, frontRightMove, backLeftMove, backLeftTurn, backRightMove, backRightTurn, 38.1, 0); + + // remember to set the joystick port + private Joystick stick = new Joystick(0); // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = @@ -50,7 +85,9 @@ private void configureBindings() { // cancelling on release. m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } - + private void defaultCommands(){ + motors.setDefaultCommand(new Drive(motors,encoders,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..ea5ea2b --- /dev/null +++ b/src/main/java/frc/robot/commands/Drive.java @@ -0,0 +1,111 @@ +package frc.robot.commands; + +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Motors; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +/** 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 Joystick joystick; + private PIDController bearingControllerFrontLeft; + private PIDController bearingControllerFrontRight; + private PIDController bearingControllerBackLeft; + private PIDController bearingControllerBackRight; + + //find a way to update the PID values + public double kP; + public double kI; + public double kD; + private double currentBearing; + private double previousBearingGoal; + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public Drive(Motors motors, Encoders encoders, Joystick stick) { + this.encoders=encoders; + this.motors=motors; + joystick=stick; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(motors,encoders); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + bearingControllerFrontLeft=new PIDController(kP, kI, kD); + bearingControllerFrontLeft.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontLeft.setTolerance(0.01745329251); + + bearingControllerFrontRight=new PIDController(kP, kI, kD); + bearingControllerFrontRight.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(0.01745329251); + + bearingControllerBackLeft=new PIDController(kP, kI, kD); + bearingControllerBackLeft.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(0.01745329251); + + bearingControllerBackRight=new PIDController(kP, kI, kD); + bearingControllerBackRight.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackRight.setTolerance(0.01745329251); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + 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(1); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),1); + + currentBearing=encoders.motorTurned(2); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),2); + + currentBearing=encoders.motorTurned(3); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),3); + + currentBearing=encoders.motorTurned(4); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),4); + //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, 1); + motors.setTurnMotors(0, 2); + motors.setTurnMotors(0, 3); + motors.setTurnMotors(0, 4); + } + + // 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..d17ffdf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -0,0 +1,111 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.Encoder; + +public class Encoders extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + public static Encoder FrontLeftMove; + public static Encoder FrontLeftTurn; + public static Encoder FrontRightMove; + public static Encoder FrontRightTurn; + public static Encoder BackLeftMove; + public static Encoder BackLeftTurn; + public static Encoder BackRightMove; + public static Encoder BackRightTurn; + //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(Encoder frontLeftMove, Encoder frontLeftTurn, Encoder frontRightTurn, Encoder frontRightMove, Encoder backLeftMove, Encoder backLeftTurn, Encoder backRightMove, Encoder backRightTurn, double motorRadius, double DistancePerPulse) { + FrontLeftMove=frontLeftMove; + FrontLeftTurn=frontLeftTurn; + FrontRightTurn=frontRightTurn; + FrontRightMove=frontRightMove; + BackLeftMove=backLeftMove; + BackLeftTurn=backLeftTurn; + BackRightMove=backRightMove; + BackRightTurn=backRightTurn; + FrontLeftMove.setDistancePerPulse(DistancePerPulse); + FrontRightMove.setDistancePerPulse(DistancePerPulse); + BackLeftMove.setDistancePerPulse(DistancePerPulse); + BackRightMove.setDistancePerPulse(DistancePerPulse); + FrontLeftTurn.setDistancePerPulse(DistancePerPulse); + FrontRightTurn.setDistancePerPulse(DistancePerPulse); + BackLeftTurn.setDistancePerPulse(DistancePerPulse); + BackRightTurn.setDistancePerPulse(DistancePerPulse); + this.motorRadius=motorRadius; + frontLeftBearing=0; + frontRightBearing=0; + backLeftBearing=0; + backRightBearing=0; + } + + public double getDistanceMoved(int motorNum){ + switch (motorNum){ + case 1: + //return frontLeft distance + return FrontLeftMove.getDistance(); + case 2: + //return frontRight distance + return FrontRightMove.getDistance(); + case 3: + //return backLeft distance + return BackLeftMove.getDistance(); + case 4: + //return backRight distance + return BackRightMove.getDistance(); + } + //this should never happen, but just in case + return 0.0; + } + public double motorTurned(int motorNum){ + switch (motorNum){ + case 1: + //return frontLeft degrees turned + distanceRotated=FrontLeftTurn.getDistance(); + //using radians + deltaRadians= distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + frontLeftBearing+=deltaRadians; + frontLeftBearing=frontLeftBearing%6.283185307; + return frontLeftBearing; + case 2: + //return frontLeft degrees turned + distanceRotated=FrontLeftTurn.getDistance(); + //using radians + deltaRadians= distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + frontRightBearing+=deltaRadians; + frontRightBearing=frontRightBearing%6.283185307; + return frontRightBearing; + case 3: + //return frontLeft degrees turned + distanceRotated=FrontLeftTurn.getDistance(); + //using radians + deltaRadians= distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + backLeftBearing+=deltaRadians; + backLeftBearing=backLeftBearing%6.283185307; + return backLeftBearing; + case 4: + //return frontLeft degrees turned + distanceRotated=FrontLeftTurn.getDistance(); + //using radians + deltaRadians= distanceRotated/motorRadius; + //if deltaRadians is more than 2pi, I am resetting it down + backRightBearing+=deltaRadians; + backRightBearing=backRightBearing%6.283185307; + 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..c925de6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +//import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.I2C; + + + +//public class Gyroscope extends SubsystemBase { + //public AHRS gyro; + //public Gyroscope(AHRS Gyro){ + // gyro=Gyro; + //} + //public double getBearing{ + //return gyro.getAngle; + //} +//} + 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..d2b8619 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.motorcontrol.Talon; + + + + +public class Motors extends SubsystemBase { + public static Talon frontLeftMove; + public static Talon frontLeftTurn; + public static Talon frontRightMove; + public static Talon frontRightTurn; + public static Talon backLeftMove; + public static Talon backLeftTurn; + public static Talon backRightMove; + public static Talon backRightTurn; + + public Motors(Talon FrontLeftMove, Talon FrontLeftTurn, Talon FrontRightMove, Talon FrontRightTurn, Talon BackLeftMove, Talon BackLeftTurn, Talon BackRightMove, Talon BackRightTurn) { + frontLeftMove=FrontLeftMove; + frontLeftTurn=FrontLeftTurn; + frontRightMove=FrontRightMove; + frontRightTurn=FrontRightTurn; + backLeftMove=BackLeftMove; + backLeftTurn=BackLeftTurn; + backRightMove=BackRightMove; + backRightTurn=BackRightTurn; + } + public void setMoveMotors(double PIDoutput){ + frontLeftMove.set(PIDoutput); + frontRightMove.set(PIDoutput); + backLeftMove.set(PIDoutput); + backRightMove.set(PIDoutput); + } + public void setTurnMotors(double PIDOutput, int motorNum){ + switch (motorNum) + { + case 1: + frontLeftTurn.set(PIDOutput); + case 2: + frontRightTurn.set(PIDOutput); + case 3: + backLeftTurn.set(PIDOutput); + case 4: + backRightTurn.set(PIDOutput); + } + } +} From aa4aa0d4d8dfb5bfef8cdc5af63a01b2ca405faa Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sun, 28 Jan 2024 17:15:55 +0000 Subject: [PATCH 02/19] Test code Code used to find out the distance per pulse of the robot --- src/main/java/frc/robot/Robot.java | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) 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 From fc7ba7d18fb172997e09c24c82adfad842b4e66c Mon Sep 17 00:00:00 2001 From: WebCoder49 Date: Wed, 31 Jan 2024 11:51:17 +0000 Subject: [PATCH 03/19] Make swerve drive code slightly more readable --- src/main/java/frc/robot/commands/Drive.java | 33 ++++++++++--------- .../java/frc/robot/subsystems/Encoders.java | 8 ++--- .../java/frc/robot/subsystems/Motors.java | 19 +++++++---- 3 files changed, 33 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index ea5ea2b..1ca46fb 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -2,6 +2,7 @@ import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.math.controller.PIDController; @@ -41,24 +42,24 @@ public Drive(Motors motors, Encoders encoders, Joystick stick) { @Override public void initialize() { bearingControllerFrontLeft=new PIDController(kP, kI, kD); - bearingControllerFrontLeft.enableContinuousInput(0, 6.283185307); + bearingControllerFrontLeft.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerFrontLeft.setTolerance(0.01745329251); + bearingControllerFrontLeft.setTolerance(Math.PI/180); bearingControllerFrontRight=new PIDController(kP, kI, kD); - bearingControllerFrontRight.enableContinuousInput(0, 6.283185307); + bearingControllerFrontRight.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerFrontRight.setTolerance(0.01745329251); + bearingControllerFrontRight.setTolerance(Math.PI/180); bearingControllerBackLeft=new PIDController(kP, kI, kD); - bearingControllerBackLeft.enableContinuousInput(0, 6.283185307); + bearingControllerBackLeft.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerBackLeft.setTolerance(0.01745329251); + bearingControllerBackLeft.setTolerance(Math.PI/180); bearingControllerBackRight=new PIDController(kP, kI, kD); - bearingControllerBackRight.enableContinuousInput(0, 6.283185307); + bearingControllerBackRight.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerBackRight.setTolerance(0.01745329251); + bearingControllerBackRight.setTolerance(Math.PI/180); } // Called every time the scheduler runs while the command is scheduled. @@ -78,16 +79,16 @@ public void execute() { bearingControllerBackRight.reset(); } currentBearing=encoders.motorTurned(1); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),1); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); currentBearing=encoders.motorTurned(2); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),2); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); currentBearing=encoders.motorTurned(3); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),3); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); currentBearing=encoders.motorTurned(4); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing),4); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_RIGHT); //setting the speed, but at 0.5 scale to ensure no one dies motors.setMoveMotors(joystick.getMagnitude()*0.5); } @@ -96,10 +97,10 @@ public void execute() { @Override public void end(boolean interrupted) { motors.setMoveMotors(0); - motors.setTurnMotors(0, 1); - motors.setTurnMotors(0, 2); - motors.setTurnMotors(0, 3); - motors.setTurnMotors(0, 4); + 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. diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index d17ffdf..8dbd0fb 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -74,7 +74,7 @@ public double motorTurned(int motorNum){ deltaRadians= distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down frontLeftBearing+=deltaRadians; - frontLeftBearing=frontLeftBearing%6.283185307; + frontLeftBearing=frontLeftBearing%(Math.PI*2); return frontLeftBearing; case 2: //return frontLeft degrees turned @@ -83,7 +83,7 @@ public double motorTurned(int motorNum){ deltaRadians= distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down frontRightBearing+=deltaRadians; - frontRightBearing=frontRightBearing%6.283185307; + frontRightBearing=frontRightBearing%(Math.PI*2); return frontRightBearing; case 3: //return frontLeft degrees turned @@ -92,7 +92,7 @@ public double motorTurned(int motorNum){ deltaRadians= distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down backLeftBearing+=deltaRadians; - backLeftBearing=backLeftBearing%6.283185307; + backLeftBearing=backLeftBearing%(Math.PI*2); return backLeftBearing; case 4: //return frontLeft degrees turned @@ -101,7 +101,7 @@ public double motorTurned(int motorNum){ deltaRadians= distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down backRightBearing+=deltaRadians; - backRightBearing=backRightBearing%6.283185307; + backRightBearing=backRightBearing%(Math.PI*2); return backRightBearing; } //this should never happen, but just so the code is happy diff --git a/src/main/java/frc/robot/subsystems/Motors.java b/src/main/java/frc/robot/subsystems/Motors.java index d2b8619..ece0884 100644 --- a/src/main/java/frc/robot/subsystems/Motors.java +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -15,7 +15,7 @@ public class Motors extends SubsystemBase { public static Talon backLeftTurn; public static Talon backRightMove; public static Talon backRightTurn; - + public Motors(Talon FrontLeftMove, Talon FrontLeftTurn, Talon FrontRightMove, Talon FrontRightTurn, Talon BackLeftMove, Talon BackLeftTurn, Talon BackRightMove, Talon BackRightTurn) { frontLeftMove=FrontLeftMove; frontLeftTurn=FrontLeftTurn; @@ -32,16 +32,21 @@ public void setMoveMotors(double PIDoutput){ backLeftMove.set(PIDoutput); backRightMove.set(PIDoutput); } - public void setTurnMotors(double PIDOutput, int motorNum){ - switch (motorNum) + + public enum TurnMotor { + FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT + } + + public void setTurnMotors(double PIDOutput, TurnMotor motor){ + switch (motor) { - case 1: + case FRONT_LEFT: frontLeftTurn.set(PIDOutput); - case 2: + case FRONT_RIGHT: frontRightTurn.set(PIDOutput); - case 3: + case BACK_LEFT: backLeftTurn.set(PIDOutput); - case 4: + case BACK_RIGHT: backRightTurn.set(PIDOutput); } } From 82a9a780683c2291e90e5d3ffd8ffeac2d2b0ebc Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Fri, 2 Feb 2024 09:33:51 +0000 Subject: [PATCH 04/19] Draft Created the code for the motors, but I haven't bound the snap function to a button yet. --- src/main/java/frc/robot/RobotContainer.java | 2 + src/main/java/frc/robot/commands/Drive.java | 30 +++- src/main/java/frc/robot/commands/Snap.java | 136 ++++++++++++++++++ .../java/frc/robot/subsystems/Encoders.java | 18 ++- 4 files changed, 174 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc/robot/commands/Snap.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 014a9d3..14a5366 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,6 +84,8 @@ private void configureBindings() { // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, // cancelling on release. m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + + } private void defaultCommands(){ motors.setDefaultCommand(new Drive(motors,encoders,stick)); diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 1ca46fb..b8e919f 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -1,11 +1,15 @@ 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 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"}) @@ -17,10 +21,14 @@ public class Drive extends Command { private PIDController bearingControllerBackLeft; private PIDController bearingControllerBackRight; - //find a way to update the PID values 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; @@ -65,6 +73,18 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + //tuning the PID constants + tab= Shuffleboard.getTab("kP"); + P=tab.add("P constant",0).getEntry(); + kP=P.getDouble(0); + tab= Shuffleboard.getTab("kI"); + I=tab.add("I constant",0).getEntry(); + kI=I.getDouble(0); + tab= Shuffleboard.getTab("kD"); + D=tab.add("D constant",0).getEntry(); + kD=D.getDouble(0); + + if(previousBearingGoal!=joystick.getDirectionRadians()) { //updating the new goal if the joystick is moved @@ -78,16 +98,16 @@ public void execute() { bearingControllerBackRight.setSetpoint(previousBearingGoal); bearingControllerBackRight.reset(); } - currentBearing=encoders.motorTurned(1); + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); - currentBearing=encoders.motorTurned(2); + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); - currentBearing=encoders.motorTurned(3); + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); - currentBearing=encoders.motorTurned(4); + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_RIGHT); //setting the speed, but at 0.5 scale to ensure no one dies motors.setMoveMotors(joystick.getMagnitude()*0.5); 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..d36985f --- /dev/null +++ b/src/main/java/frc/robot/commands/Snap.java @@ -0,0 +1,136 @@ +package frc.robot.commands; + +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +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 Joystick stick; + + 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 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, Joystick stick) { + this.motors = motors; + this.encoders= encoders; + this.stick= stick; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(motors, encoders); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + bearingControllerFrontLeft=new PIDController(kP, kI, kD); + bearingControllerFrontLeft.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontLeft.setTolerance(0.01745329251); + + bearingControllerFrontRight=new PIDController(kP, kI, kD); + bearingControllerFrontRight.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(0.01745329251); + + bearingControllerBackLeft=new PIDController(kP, kI, kD); + bearingControllerBackLeft.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(0.01745329251); + + bearingControllerBackRight=new PIDController(kP, kI, kD); + bearingControllerBackRight.enableContinuousInput(0, 6.283185307); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackRight.setTolerance(0.01745329251); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { +//tuning the PID constants + tab= Shuffleboard.getTab("kP"); + P=tab.add("P constant",0).getEntry(); + kP=P.getDouble(0); + tab= Shuffleboard.getTab("kI"); + I=tab.add("I constant",0).getEntry(); + kI=I.getDouble(0); + tab= Shuffleboard.getTab("kD"); + D=tab.add("D constant",0).getEntry(); + kD=D.getDouble(0); + + joystickBearing=stick.getDirectionRadians(); + // coding the data from 1 to 4 + joystickBearing= joystickBearing*0.63661977236; + //converting back to radians + snapGoal=Math.round(joystickBearing)*1.570796326; + if (snapGoal!=previousSnapGoal){ + //updating the new goal if the joystick is moved + bearingControllerFrontLeft.setSetpoint(snapGoal); + bearingControllerFrontLeft.reset(); + bearingControllerFrontRight.setSetpoint(snapGoal); + bearingControllerFrontRight.reset(); + bearingControllerBackLeft.setSetpoint(snapGoal); + bearingControllerBackLeft.reset(); + bearingControllerBackRight.setSetpoint(snapGoal); + bearingControllerBackRight.reset(); + previousSnapGoal=snapGoal; + } + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_RIGHT); + } + + + // 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 index 8dbd0fb..d47f0c0 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -46,7 +46,6 @@ public Encoders(Encoder frontLeftMove, Encoder frontLeftTurn, Encoder frontRight backLeftBearing=0; backRightBearing=0; } - public double getDistanceMoved(int motorNum){ switch (motorNum){ case 1: @@ -65,9 +64,14 @@ public double getDistanceMoved(int motorNum){ //this should never happen, but just in case return 0.0; } - public double motorTurned(int motorNum){ - switch (motorNum){ - case 1: + + public enum TurnEncoder { + FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT + } + + public double motorTurned(TurnEncoder motorType){ + switch (motorType){ + case FRONT_LEFT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians @@ -76,7 +80,7 @@ public double motorTurned(int motorNum){ frontLeftBearing+=deltaRadians; frontLeftBearing=frontLeftBearing%(Math.PI*2); return frontLeftBearing; - case 2: + case FRONT_RIGHT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians @@ -85,7 +89,7 @@ public double motorTurned(int motorNum){ frontRightBearing+=deltaRadians; frontRightBearing=frontRightBearing%(Math.PI*2); return frontRightBearing; - case 3: + case BACK_LEFT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians @@ -94,7 +98,7 @@ public double motorTurned(int motorNum){ backLeftBearing+=deltaRadians; backLeftBearing=backLeftBearing%(Math.PI*2); return backLeftBearing; - case 4: + case BACK_RIGHT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians From 2bcab8d67ab3042b349b2bbf783c2cd6b5dd86f8 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sun, 4 Feb 2024 13:44:40 +0000 Subject: [PATCH 05/19] Code for the snap Command It is bound to button 1 on the Joystick --- src/main/java/frc/robot/RobotContainer.java | 9 +++++++-- src/main/java/frc/robot/commands/Snap.java | 6 +++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14a5366..61048c6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,10 +9,12 @@ 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 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; @@ -56,6 +58,8 @@ public class RobotContainer { // remember to set the joystick port private Joystick stick = new Joystick(0); + // 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 = @@ -65,6 +69,7 @@ public class RobotContainer { public RobotContainer() { // Configure the trigger bindings configureBindings(); + defaultCommands(); } /** @@ -84,8 +89,8 @@ private void configureBindings() { // 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, stick)); + } private void defaultCommands(){ motors.setDefaultCommand(new Drive(motors,encoders,stick)); diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index d36985f..d5e0fcb 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -109,13 +109,13 @@ public void execute() { motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_RIGHT); + motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); } From b663fd74c09e9c70d7ea9c5641f0a66c9c2615ca Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 5 Feb 2024 19:14:07 +0000 Subject: [PATCH 06/19] New commit Includes code so that the robot can update its PID. Also allows the robot to offset the encoders such that they will be field oriented (assuming that the robot starts facing the field) --- src/main/java/frc/robot/RobotContainer.java | 28 ++++++------- src/main/java/frc/robot/commands/Drive.java | 40 +++++++++++++++---- .../java/frc/robot/subsystems/Encoders.java | 15 ++++--- .../java/frc/robot/subsystems/Gyroscope.java | 25 +++++++----- .../java/frc/robot/subsystems/Motors.java | 2 + vendordeps/NavX.json | 40 +++++++++++++++++++ 6 files changed, 111 insertions(+), 39 deletions(-) create mode 100644 vendordeps/NavX.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 014a9d3..3a3f020 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,19 +4,22 @@ package frc.robot; -import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.DriveConstants; // 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.subsystems.ExampleSubsystem; import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Gyroscope; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.I2C; +import com.kauailabs.navx.frc.AHRS; + import edu.wpi.first.wpilibj.Encoder; /** @@ -48,23 +51,24 @@ public class RobotContainer { private final Encoder backLeftTurn = new Encoder(0,0); private final Encoder backRightMove = new Encoder(0, 0); private final Encoder backRightTurn = new Encoder(0,0); + 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 pulse is still unkown private final Encoders encoders= new Encoders(frontLeftMove, frontLeftTurn, frontRightTurn, frontRightMove, backLeftMove, backLeftTurn, backRightMove, backRightTurn, 38.1, 0); + private final Gyroscope gyroSubsystem = new Gyroscope(gyro); + - // remember to set the joystick port - private Joystick stick = new Joystick(0); + private Joystick stick = new Joystick(DriveConstants.joystickPort); + - // 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(); } /** @@ -77,16 +81,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(){ - motors.setDefaultCommand(new Drive(motors,encoders,stick)); + motors.setDefaultCommand(new Drive(motors,encoders,gyroSubsystem,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 index 1ca46fb..143c49a 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -1,28 +1,39 @@ 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.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; - //find a way to update the PID values 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; /** @@ -30,17 +41,29 @@ public class Drive extends Command { * * @param subsystem The subsystem used by this command. */ - public Drive(Motors motors, Encoders encoders, Joystick stick) { + 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); + 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=P.getDouble(0); + tab= Shuffleboard.getTab("kI"); + I=tab.add("I constant",0).getEntry(); + kI=I.getDouble(0); + tab= Shuffleboard.getTab("kD"); + D=tab.add("D constant",0).getEntry(); + kD=D.getDouble(0); bearingControllerFrontLeft=new PIDController(kP, kI, kD); bearingControllerFrontLeft.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) @@ -65,10 +88,11 @@ public void initialize() { // 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(); + previousBearingGoal=joystick.getDirectionRadians()+fieldOrientOffset; bearingControllerFrontLeft.setSetpoint(previousBearingGoal); bearingControllerFrontLeft.reset(); bearingControllerFrontRight.setSetpoint(previousBearingGoal); @@ -78,16 +102,16 @@ public void execute() { bearingControllerBackRight.setSetpoint(previousBearingGoal); bearingControllerBackRight.reset(); } - currentBearing=encoders.motorTurned(1); + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); - currentBearing=encoders.motorTurned(2); + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); - currentBearing=encoders.motorTurned(3); + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); - currentBearing=encoders.motorTurned(4); + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_RIGHT); //setting the speed, but at 0.5 scale to ensure no one dies motors.setMoveMotors(joystick.getMagnitude()*0.5); diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index 8dbd0fb..4542784 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -65,9 +65,12 @@ public double getDistanceMoved(int motorNum){ //this should never happen, but just in case return 0.0; } - public double motorTurned(int motorNum){ - switch (motorNum){ - case 1: + 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.getDistance(); //using radians @@ -76,7 +79,7 @@ public double motorTurned(int motorNum){ frontLeftBearing+=deltaRadians; frontLeftBearing=frontLeftBearing%(Math.PI*2); return frontLeftBearing; - case 2: + case FRONT_RIGHT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians @@ -85,7 +88,7 @@ public double motorTurned(int motorNum){ frontRightBearing+=deltaRadians; frontRightBearing=frontRightBearing%(Math.PI*2); return frontRightBearing; - case 3: + case BACK_LEFT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians @@ -94,7 +97,7 @@ public double motorTurned(int motorNum){ backLeftBearing+=deltaRadians; backLeftBearing=backLeftBearing%(Math.PI*2); return backLeftBearing; - case 4: + case BACK_RIGHT: //return frontLeft degrees turned distanceRotated=FrontLeftTurn.getDistance(); //using radians diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java index c925de6..1841aa8 100644 --- a/src/main/java/frc/robot/subsystems/Gyroscope.java +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -2,18 +2,21 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; -//import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.I2C; +import com.kauailabs.navx.frc.AHRS; -//public class Gyroscope extends SubsystemBase { - //public AHRS gyro; - //public Gyroscope(AHRS Gyro){ - // gyro=Gyro; - //} - //public double getBearing{ - //return gyro.getAngle; - //} -//} +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 index ece0884..411d09c 100644 --- a/src/main/java/frc/robot/subsystems/Motors.java +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.motorcontrol.Talon; @@ -15,6 +16,7 @@ public class Motors extends SubsystemBase { public static Talon backLeftTurn; public static Talon backRightMove; public static Talon backRightTurn; + public Motors(Talon FrontLeftMove, Talon FrontLeftTurn, Talon FrontRightMove, Talon FrontRightTurn, Talon BackLeftMove, Talon BackLeftTurn, Talon BackRightMove, Talon BackRightTurn) { frontLeftMove=FrontLeftMove; 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 From bfa784f57648ea2a236d5c37c99ba77f8ddab223 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 5 Feb 2024 20:02:44 +0000 Subject: [PATCH 07/19] This should work --- src/main/java/frc/robot/commands/Drive.java | 22 +-- src/main/java/frc/robot/commands/Snap.java | 139 +++++++++++------- .../java/frc/robot/subsystems/Gyroscope.java | 26 ++-- vendordeps/NavX.json | 40 +++++ 4 files changed, 153 insertions(+), 74 deletions(-) create mode 100644 vendordeps/NavX.json diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index b8e919f..4e67ac8 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -49,6 +49,17 @@ public Drive(Motors motors, Encoders encoders, Joystick stick) { // 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=P.getDouble(0); + tab= Shuffleboard.getTab("kI"); + I=tab.add("I constant",0).getEntry(); + kI=I.getDouble(0); + tab= Shuffleboard.getTab("kD"); + D=tab.add("D constant",0).getEntry(); + kD=D.getDouble(0); + bearingControllerFrontLeft=new PIDController(kP, kI, kD); bearingControllerFrontLeft.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) @@ -73,16 +84,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //tuning the PID constants - tab= Shuffleboard.getTab("kP"); - P=tab.add("P constant",0).getEntry(); - kP=P.getDouble(0); - tab= Shuffleboard.getTab("kI"); - I=tab.add("I constant",0).getEntry(); - kI=I.getDouble(0); - tab= Shuffleboard.getTab("kD"); - D=tab.add("D constant",0).getEntry(); - kD=D.getDouble(0); + if(previousBearingGoal!=joystick.getDirectionRadians()) diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index d5e0fcb..4c827c6 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -2,9 +2,9 @@ import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Encoders.TurnEncoder; -import frc.robot.subsystems.ExampleSubsystem; 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; @@ -17,20 +17,27 @@ 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 double kP; - public double kI; - public double kD; 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; @@ -42,83 +49,109 @@ public class Snap extends Command { * * @param subsystem The subsystem used by this command. */ - public Snap(Motors motors, Encoders encoders, Joystick stick) { + 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); + 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, 6.283185307); + bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerFrontLeft.setTolerance(0.01745329251); + bearingControllerFrontLeft.setTolerance(Math.PI/360); + bearingControllerFrontLeft.setSetpoint(Math.PI/4); bearingControllerFrontRight=new PIDController(kP, kI, kD); - bearingControllerFrontRight.enableContinuousInput(0, 6.283185307); + bearingControllerFrontRight.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerFrontRight.setTolerance(0.01745329251); + bearingControllerFrontRight.setTolerance(Math.PI/360); + bearingControllerFrontRight.setSetpoint(Math.PI*3/4); bearingControllerBackLeft=new PIDController(kP, kI, kD); - bearingControllerBackLeft.enableContinuousInput(0, 6.283185307); + bearingControllerBackLeft.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerBackLeft.setTolerance(0.01745329251); + bearingControllerBackLeft.setTolerance(Math.PI/360); + bearingControllerBackLeft.setSetpoint(Math.PI*5/4); bearingControllerBackRight=new PIDController(kP, kI, kD); - bearingControllerBackRight.enableContinuousInput(0, 6.283185307); + bearingControllerBackRight.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=1 degree (radians equivalent) - bearingControllerBackRight.setTolerance(0.01745329251); + 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() { -//tuning the PID constants - tab= Shuffleboard.getTab("kP"); - P=tab.add("P constant",0).getEntry(); - kP=P.getDouble(0); - tab= Shuffleboard.getTab("kI"); - I=tab.add("I constant",0).getEntry(); - kI=I.getDouble(0); - tab= Shuffleboard.getTab("kD"); - D=tab.add("D constant",0).getEntry(); - kD=D.getDouble(0); + //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); - joystickBearing=stick.getDirectionRadians(); - // coding the data from 1 to 4 - joystickBearing= joystickBearing*0.63661977236; - //converting back to radians - snapGoal=Math.round(joystickBearing)*1.570796326; - if (snapGoal!=previousSnapGoal){ - //updating the new goal if the joystick is moved - bearingControllerFrontLeft.setSetpoint(snapGoal); - bearingControllerFrontLeft.reset(); - bearingControllerFrontRight.setSetpoint(snapGoal); - bearingControllerFrontRight.reset(); - bearingControllerBackLeft.setSetpoint(snapGoal); - bearingControllerBackLeft.reset(); - bearingControllerBackRight.setSetpoint(snapGoal); - bearingControllerBackRight.reset(); - previousSnapGoal=snapGoal; + 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())); } - 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); } - - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java index c925de6..6c512af 100644 --- a/src/main/java/frc/robot/subsystems/Gyroscope.java +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -2,18 +2,22 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; -//import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.I2C; +import com.kauailabs.navx.frc.AHRS; -//public class Gyroscope extends SubsystemBase { - //public AHRS gyro; - //public Gyroscope(AHRS Gyro){ - // gyro=Gyro; - //} - //public double getBearing{ - //return gyro.getAngle; - //} -//} +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/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 From 696ec53870fe305916152166111bd4af65c30966 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 5 Feb 2024 20:05:48 +0000 Subject: [PATCH 08/19] Sorry Forgot to update the robotContainer --- src/main/java/frc/robot/RobotContainer.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61048c6..78d46a4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; +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; @@ -20,6 +21,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.Encoder; +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 @@ -50,14 +53,15 @@ public class RobotContainer { private final Encoder backLeftTurn = new Encoder(0,0); private final Encoder backRightMove = new Encoder(0, 0); private final Encoder backRightTurn = new Encoder(0,0); + 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 pulse is still unkown private final Encoders encoders= new Encoders(frontLeftMove, frontLeftTurn, frontRightTurn, frontRightMove, backLeftMove, backLeftTurn, backRightMove, backRightTurn, 38.1, 0); - + private final Gyroscope gyroscope = new Gyroscope(gyro); // remember to set the joystick port - private Joystick stick = new Joystick(0); + private Joystick stick = new Joystick(OperatorConstants.kDriverControllerPort); // this is the button on the handle of the joystick private JoystickButton snapButton = new JoystickButton(stick, 1); @@ -89,7 +93,7 @@ private void configureBindings() { // 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, stick)); + snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick)); } private void defaultCommands(){ From 27fa5df1134532b7a96cf2618a7327e03b4dfd71 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 7 Feb 2024 15:55:16 +0000 Subject: [PATCH 09/19] new update to use the proper motors --- src/main/java/frc/robot/RobotContainer.java | 35 +++++----- vendordeps/REVLib.json | 74 +++++++++++++++++++++ 2 files changed, 91 insertions(+), 18 deletions(-) create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3a3f020..1b994d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,11 +16,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj.I2C; -import com.kauailabs.navx.frc.AHRS; - +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.Encoder; +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 @@ -35,14 +35,14 @@ public class RobotContainer { // variables are // remember to configure the acutal channels - private final Talon FrontLeftMove= new Talon(0); - private final Talon FrontLeftTurn= new Talon(2); - private final Talon FrontRightMove= new Talon(3); - private final Talon FrontRightTurn= new Talon(4); - private final Talon BackLeftMove= new Talon(5); - private final Talon BackLeftTurn = new Talon(6); - private final Talon BackRightMove= new Talon(7); - private final Talon BackRightTurn= new Talon(8); + private final CANSparkMax FrontLeftMove= new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax FrontLeftTurn= new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax FrontRightMove= new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax FrontRightTurn= new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax BackLeftMove= new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax BackLeftTurn = new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax BackRightMove= new CANSparkMax(0,MotorType.kBrushless); + private final CANSparkMax BackRightTurn= new CANSparkMax(0,MotorType.kBrushless); private final Encoder frontLeftMove = new Encoder(0, 0); private final Encoder frontLeftTurn = new Encoder(0,0); private final Encoder frontRightMove = new Encoder(0, 0); @@ -57,10 +57,10 @@ public class RobotContainer { private final Motors motors= new Motors(FrontLeftMove, FrontLeftTurn, FrontRightMove, FrontRightTurn, BackLeftMove, BackLeftTurn, BackRightMove, BackRightTurn); //motor radius is configured in mm and distance per pulse is still unkown private final Encoders encoders= new Encoders(frontLeftMove, frontLeftTurn, frontRightTurn, frontRightMove, backLeftMove, backLeftTurn, backRightMove, backRightTurn, 38.1, 0); - private final Gyroscope gyroSubsystem = new Gyroscope(gyro); - + private final Gyroscope gyroscope = new Gyroscope(gyro); + // remember to set the joystick port + private Joystick stick = new Joystick(0); - private Joystick stick = new Joystick(DriveConstants.joystickPort); @@ -81,12 +81,11 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - - + } private void defaultCommands(){ - motors.setDefaultCommand(new Drive(motors,encoders,gyroSubsystem,stick)); + motors.setDefaultCommand(new Drive(motors,encoders,gyroscope,stick)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. 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 From 159ddf7eeddb06a88f90f900db06e9e8546138e0 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 7 Feb 2024 15:55:34 +0000 Subject: [PATCH 10/19] change to the motor subsystem --- .../java/frc/robot/subsystems/Motors.java | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Motors.java b/src/main/java/frc/robot/subsystems/Motors.java index 411d09c..0c02915 100644 --- a/src/main/java/frc/robot/subsystems/Motors.java +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -2,23 +2,23 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.motorcontrol.Talon; +import com.revrobotics.CANSparkMax; public class Motors extends SubsystemBase { - public static Talon frontLeftMove; - public static Talon frontLeftTurn; - public static Talon frontRightMove; - public static Talon frontRightTurn; - public static Talon backLeftMove; - public static Talon backLeftTurn; - public static Talon backRightMove; - public static Talon backRightTurn; + 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(Talon FrontLeftMove, Talon FrontLeftTurn, Talon FrontRightMove, Talon FrontRightTurn, Talon BackLeftMove, Talon BackLeftTurn, Talon BackRightMove, Talon 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; @@ -27,12 +27,14 @@ public Motors(Talon FrontLeftMove, Talon FrontLeftTurn, Talon FrontRightMove, Ta 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); - frontRightMove.set(PIDoutput); - backLeftMove.set(PIDoutput); - backRightMove.set(PIDoutput); } public enum TurnMotor { From 9ad1b19f1f76a5f7b8f2cefb49b32e1b9f60eaf4 Mon Sep 17 00:00:00 2001 From: WebCoder49 Date: Wed, 7 Feb 2024 16:16:13 +0000 Subject: [PATCH 11/19] Get PID from constants for now --- networktables.json | 1 + simgui-ds.json | 92 +++++++++++++++++++++ simgui.json | 10 +++ src/main/java/frc/robot/Constants.java | 6 ++ src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/Drive.java | 27 +++--- 6 files changed, 123 insertions(+), 14 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..5f9d275 --- /dev/null +++ b/simgui.json @@ -0,0 +1,10 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..e4048dd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,4 +16,10 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; } + + public static class PIDConstants { + public static final double kDrivetrainP = 1.0; + public static final double kDrivetrainI = 0.0; + public static final double kDrivetrainD = 0.0; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b994d8..1eb1f0a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,6 @@ package frc.robot; -import frc.robot.Constants.DriveConstants; // here is where you put all your commands and subsystems; import frc.robot.commands.Autos; import frc.robot.commands.Drive; diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 143c49a..b44e3e5 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -5,6 +5,7 @@ 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; @@ -26,10 +27,10 @@ public class Drive extends Command { public double kP; public double kI; public double kD; - public ShuffleboardTab tab; - public GenericEntry P; - public GenericEntry I; - public GenericEntry D; + // public ShuffleboardTab tab; + // public GenericEntry P; + // public GenericEntry I; + // public GenericEntry D; private double currentBearing; private double previousBearingGoal; @@ -55,15 +56,15 @@ public Drive(Motors motors, Encoders encoders, Gyroscope gyro, Joystick stick) { @Override public void initialize() { //tuning the PID constants - tab= Shuffleboard.getTab("kP"); - P=tab.add("P constant",0).getEntry(); - kP=P.getDouble(0); - tab= Shuffleboard.getTab("kI"); - I=tab.add("I constant",0).getEntry(); - kI=I.getDouble(0); - tab= Shuffleboard.getTab("kD"); - D=tab.add("D constant",0).getEntry(); - kD=D.getDouble(0); + // 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) From b87c9b24a492c7596fb7baeee115c23605fa8ab5 Mon Sep 17 00:00:00 2001 From: personofalltime <66888981+personofalltime@users.noreply.github.com> Date: Thu, 8 Feb 2024 17:51:32 +0000 Subject: [PATCH 12/19] CAN IDs Assigned --- src/main/java/frc/robot/Constants.java | 11 +++++++++++ src/main/java/frc/robot/RobotContainer.java | 18 ++++++++++-------- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e4048dd..7f48304 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,17 @@ * 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 OperatorConstants { public static final int kDriverControllerPort = 0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 66f41c4..3dfa670 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,8 @@ import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; +import frc.robot.Constants.Ports; +import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.Gyroscope; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -36,14 +38,14 @@ public class RobotContainer { // variables are // remember to configure the acutal channels - private final CANSparkMax FrontLeftMove= new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax FrontLeftTurn= new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax FrontRightMove= new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax FrontRightTurn= new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax BackLeftMove= new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax BackLeftTurn = new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax BackRightMove= new CANSparkMax(0,MotorType.kBrushless); - private final CANSparkMax BackRightTurn= new CANSparkMax(0,MotorType.kBrushless); + 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 Encoder frontLeftMove = new Encoder(0, 0); private final Encoder frontLeftTurn = new Encoder(0,0); private final Encoder frontRightMove = new Encoder(0, 0); From e88d3b126eb2f9085757e98caf59f7e2040a184d Mon Sep 17 00:00:00 2001 From: WebCoder49 Date: Fri, 9 Feb 2024 13:55:03 +0000 Subject: [PATCH 13/19] Get encoders from SparkMAX; fix some naming conventions --- src/main/java/frc/robot/RobotContainer.java | 39 +++--- .../java/frc/robot/subsystems/Encoders.java | 114 ++++++++++-------- 2 files changed, 83 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3dfa670..b69aa72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ 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 edu.wpi.first.wpilibj.Encoder; import com.kauailabs.navx.frc.AHRS; @@ -38,28 +39,28 @@ public class RobotContainer { // 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 Encoder frontLeftMove = new Encoder(0, 0); - private final Encoder frontLeftTurn = new Encoder(0,0); - private final Encoder frontRightMove = new Encoder(0, 0); - private final Encoder frontRightTurn = new Encoder(0,0); - private final Encoder backLeftMove = new Encoder(0, 0); - private final Encoder backLeftTurn = new Encoder(0,0); - private final Encoder backRightMove = new Encoder(0, 0); - private final Encoder backRightTurn = new Encoder(0,0); + 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 pulse is still unkown - private final Encoders encoders= new Encoders(frontLeftMove, frontLeftTurn, frontRightTurn, frontRightMove, backLeftMove, backLeftTurn, backRightMove, backRightTurn, 38.1, 0); + 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, 0); private final Gyroscope gyroscope = new Gyroscope(gyro); // remember to set the joystick port diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index 5171b06..d5a649f 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -1,18 +1,19 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.Encoder; + +import com.revrobotics.RelativeEncoder; public class Encoders extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - public static Encoder FrontLeftMove; - public static Encoder FrontLeftTurn; - public static Encoder FrontRightMove; - public static Encoder FrontRightTurn; - public static Encoder BackLeftMove; - public static Encoder BackLeftTurn; - public static Encoder BackRightMove; - public static Encoder BackRightTurn; + 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 degrees turned public double motorRadius; public double distanceRotated; @@ -23,43 +24,54 @@ public class Encoders extends SubsystemBase { public double backLeftBearing; public double backRightBearing; - public Encoders(Encoder frontLeftMove, Encoder frontLeftTurn, Encoder frontRightTurn, Encoder frontRightMove, Encoder backLeftMove, Encoder backLeftTurn, Encoder backRightMove, Encoder backRightTurn, double motorRadius, double DistancePerPulse) { - FrontLeftMove=frontLeftMove; - FrontLeftTurn=frontLeftTurn; - FrontRightTurn=frontRightTurn; - FrontRightMove=frontRightMove; - BackLeftMove=backLeftMove; - BackLeftTurn=backLeftTurn; - BackRightMove=backRightMove; - BackRightTurn=backRightTurn; - FrontLeftMove.setDistancePerPulse(DistancePerPulse); - FrontRightMove.setDistancePerPulse(DistancePerPulse); - BackLeftMove.setDistancePerPulse(DistancePerPulse); - BackRightMove.setDistancePerPulse(DistancePerPulse); - FrontLeftTurn.setDistancePerPulse(DistancePerPulse); - FrontRightTurn.setDistancePerPulse(DistancePerPulse); - BackLeftTurn.setDistancePerPulse(DistancePerPulse); - BackRightTurn.setDistancePerPulse(DistancePerPulse); - this.motorRadius=motorRadius; - frontLeftBearing=0; - frontRightBearing=0; - backLeftBearing=0; - backRightBearing=0; + public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, RelativeEncoder frontRightTurn, RelativeEncoder frontRightMove, RelativeEncoder backLeftMove, RelativeEncoder backLeftTurn, RelativeEncoder backRightMove, RelativeEncoder backRightTurn, double motorRadius, double distancePerRotation) { + 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); + + frontLeftMove.setPositionConversionFactor(distancePerRotation); + frontRightMove.setPositionConversionFactor(distancePerRotation); + backLeftMove.setPositionConversionFactor(distancePerRotation); + backRightMove.setPositionConversionFactor(distancePerRotation); + frontLeftTurn.setPositionConversionFactor(distancePerRotation); + frontRightTurn.setPositionConversionFactor(distancePerRotation); + backLeftTurn.setPositionConversionFactor(distancePerRotation); + backRightTurn.setPositionConversionFactor(distancePerRotation); + this.motorRadius = motorRadius; + frontLeftBearing = 0; + frontRightBearing = 0; + backLeftBearing = 0; + backRightBearing = 0; } public double getDistanceMoved(int motorNum){ switch (motorNum){ case 1: //return frontLeft distance - return FrontLeftMove.getDistance(); + return frontLeftMove.getPosition(); case 2: //return frontRight distance - return FrontRightMove.getDistance(); + return frontRightMove.getPosition(); case 3: //return backLeft distance - return BackLeftMove.getDistance(); + return backLeftMove.getPosition(); case 4: //return backRight distance - return BackRightMove.getDistance(); + return backRightMove.getPosition(); } //this should never happen, but just in case return 0.0; @@ -71,39 +83,39 @@ public double motorTurned(TurnEncoder encoder){ switch (encoder){ case FRONT_LEFT: //return frontLeft degrees turned - distanceRotated=FrontLeftTurn.getDistance(); + distanceRotated = frontLeftTurn.getPosition(); //using radians - deltaRadians= distanceRotated/motorRadius; + deltaRadians = distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down - frontLeftBearing+=deltaRadians; - frontLeftBearing=frontLeftBearing%(Math.PI*2); + frontLeftBearing += deltaRadians; + frontLeftBearing = frontLeftBearing%(Math.PI*2); return frontLeftBearing; case FRONT_RIGHT: //return frontLeft degrees turned - distanceRotated=FrontLeftTurn.getDistance(); + distanceRotated = frontLeftTurn.getPosition(); //using radians - deltaRadians= distanceRotated/motorRadius; + deltaRadians = distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down - frontRightBearing+=deltaRadians; - frontRightBearing=frontRightBearing%(Math.PI*2); + frontRightBearing += deltaRadians; + frontRightBearing = frontRightBearing%(Math.PI*2); return frontRightBearing; case BACK_LEFT: //return frontLeft degrees turned - distanceRotated=FrontLeftTurn.getDistance(); + distanceRotated = frontLeftTurn.getPosition(); //using radians - deltaRadians= distanceRotated/motorRadius; + deltaRadians = distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down - backLeftBearing+=deltaRadians; - backLeftBearing=backLeftBearing%(Math.PI*2); + backLeftBearing += deltaRadians; + backLeftBearing = backLeftBearing%(Math.PI*2); return backLeftBearing; case BACK_RIGHT: //return frontLeft degrees turned - distanceRotated=FrontLeftTurn.getDistance(); + distanceRotated = frontLeftTurn.getPosition(); //using radians - deltaRadians= distanceRotated/motorRadius; + deltaRadians = distanceRotated/motorRadius; //if deltaRadians is more than 2pi, I am resetting it down - backRightBearing+=deltaRadians; - backRightBearing=backRightBearing%(Math.PI*2); + backRightBearing += deltaRadians; + backRightBearing = backRightBearing%(Math.PI*2); return backRightBearing; } //this should never happen, but just so the code is happy From 467e0f798980cb932d90ac432559a79f166da2ac Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 12 Feb 2024 14:04:49 +0000 Subject: [PATCH 14/19] The base subsystem and command required for this task I haven't actually made any usable code --- src/main/java/frc/robot/Constants.java | 16 ++ src/main/java/frc/robot/RobotContainer.java | 17 +- .../java/frc/robot/commands/AlignAmpShot.java | 150 ++++++++++++++++++ .../frc/robot/subsystems/TagDetector.java | 49 ++++++ 4 files changed, 227 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AlignAmpShot.java create mode 100644 src/main/java/frc/robot/subsystems/TagDetector.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7f48304..4cf7d12 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,6 +26,8 @@ public static class Ports { public static class OperatorConstants { public static final int kDriverControllerPort = 0; + public static final int snapButton=1; + public static final int AmpShotButton=2; } public static class PIDConstants { @@ -33,4 +35,18 @@ public static class PIDConstants { public static final double kDrivetrainI = 0.0; public static final double kDrivetrainD = 0.0; } + public static class AprilTags{ + //this stores the settings for the april tag detector config and the families to be detected by the detector + //remember to actually configure these + public static final int numThreads=1; + public static final float quadDecimate=2; + public static final float quadSigma=0; + + //https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html + public static final String family="36h11"; + + //this also stores the april tag numbers of all the major destinations + //remember to actually fill this in with the correct april tags + public static final int ampNum=0; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b69aa72..ef0e5d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import frc.robot.commands.AlignAmpShot; // here is where you put all your commands and subsystems; import frc.robot.commands.Autos; import frc.robot.commands.Drive; @@ -11,18 +12,20 @@ import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Gyroscope; +import frc.robot.subsystems.TagDetector; import frc.robot.Constants.Ports; 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 edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagDetector.Config; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.wpilibj.Encoder; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.I2C; @@ -56,17 +59,22 @@ public class RobotContainer { private final RelativeEncoder backRightMoveEncoder = backRightMove.getEncoder(); private final RelativeEncoder backRightTurnEncoder = backRightTurn.getEncoder(); private final AHRS gyro = new AHRS(I2C.Port.kMXP); + private final AprilTagDetector detector= new AprilTagDetector(); + private final AprilTagDetector.Config configuration= new Config(); 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, 0); private final Gyroscope gyroscope = new Gyroscope(gyro); + private final TagDetector tagDetector= new TagDetector(detector, configuration); + // 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); + private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButton); + private JoystickButton ampShotButton= new JoystickButton(stick, OperatorConstants.AmpShotButton); @@ -87,9 +95,8 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - - snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick)); + ampShotButton.whileTrue(new AlignAmpShot(encoders, motors, tagDetector)); } private void defaultCommands(){ diff --git a/src/main/java/frc/robot/commands/AlignAmpShot.java b/src/main/java/frc/robot/commands/AlignAmpShot.java new file mode 100644 index 0000000..66bca1e --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignAmpShot.java @@ -0,0 +1,150 @@ +// 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.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +import frc.robot.subsystems.TagDetector; +import frc.robot.Constants.AprilTags; + +import javax.swing.text.html.HTML.Tag; + +import edu.wpi.first.apriltag.AprilTag; + +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.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class AlignAmpShot extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Encoders encoders; + private final Motors motors; + private final TagDetector tagDetector; + + private PIDController bearingControllerFrontLeft; + private PIDController bearingControllerFrontRight; + private PIDController bearingControllerBackLeft; + private PIDController bearingControllerBackRight; + private PIDController robotBearingController; + private PIDController robotMovementController; + + 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 kPRobotTurn; + public double kIRobotTurn; + public double kDRobotTurn; + public double kPRobotMove; + public double kIRobotMove; + public double kDRobotMove; + + public int tag; + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AlignAmpShot(Encoders encoders, Motors motors, TagDetector tagDetector) { + this.encoders=encoders; + this.motors=motors; + this.tagDetector=tagDetector; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(encoders,motors,tagDetector); + } + + // 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("kPRobotTurn"); + P=tab.add("Robot P constant",0).getEntry(); + kPRobotTurn=P.getDouble(0); + tab= Shuffleboard.getTab("kIRobotTurn"); + I=tab.add("Robot I constant",0).getEntry(); + kIRobotTurn=I.getDouble(0); + tab= Shuffleboard.getTab("kDRobotTurn"); + D=tab.add("Robot D constant",0).getEntry(); + kDRobotTurn=D.getDouble(0); + + tab= Shuffleboard.getTab("kPRobotMove"); + P=tab.add("Robot P constant",0).getEntry(); + kDRobotMove=P.getDouble(0); + tab= Shuffleboard.getTab("kIRobotMove"); + I=tab.add("Robot I constant",0).getEntry(); + kIRobotMove=I.getDouble(0); + tab= Shuffleboard.getTab("kDRobotMove"); + D=tab.add("Robot D constant",0).getEntry(); + kPRobotMove=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); + + bearingControllerFrontRight=new PIDController(kP, kI, kD); + bearingControllerFrontRight.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(Math.PI/360); + + bearingControllerBackLeft=new PIDController(kP, kI, kD); + bearingControllerBackLeft.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(Math.PI/360); + + bearingControllerBackRight=new PIDController(kP, kI, kD); + bearingControllerBackRight.enableContinuousInput(0, 2*Math.PI); + //setting tolerance to +=1 degree (radians equivalent) + bearingControllerBackRight.setTolerance(Math.PI/360); + + //the PID controller for the robot itself after turning mode has been achieved + robotBearingController= new PIDController(kPRobotTurn, kIRobotTurn, kDRobotTurn); + robotBearingController.enableContinuousInput(0, 2*Math.PI); + robotBearingController.setTolerance(Math.PI/360); + + //the PID controller for the robot so that it can move into position + robotMovementController= new PIDController(kDRobotMove, kIRobotMove, kDRobotMove); + //temporarily setting the movement controller to 1cm. + robotMovementController.setTolerance(0.01); + + tag=AprilTags.ampNum; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/TagDetector.java b/src/main/java/frc/robot/subsystems/TagDetector.java new file mode 100644 index 0000000..08b93ff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TagDetector.java @@ -0,0 +1,49 @@ +// 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.subsystems; +import frc.robot.Constants.AprilTags; + +import edu.wpi.first.apriltag.AprilTagDetector; + + +import org.opencv.core.Mat; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class TagDetector extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + public final AprilTagDetector detector; + public final AprilTagDetector.Config config; + public AprilTagDetection detection; + public TagDetector(AprilTagDetector Detector, AprilTagDetector.Config configuration) { + detector=Detector; + detector.addFamily(AprilTags.family); + config=configuration; + config.numThreads=AprilTags.numThreads; + config.quadDecimate=AprilTags.quadDecimate; + config.quadSigma=AprilTags.quadSigma; + detector.setConfig(config); + } + public boolean updateDetection(Mat image, int tagID){ + //this code inherently assumes that there is only 1 april tag on screen at any given point in + //time, which I suspect will be true for most cases + if (detector.detect(image)!=null){ + detection=detector.detect(image)[0]; + if (tagID==detection.getId()){ + //essentially return true if the correct april tag is found + return true; + } + else{ + return false; + } + } + return false; + + } + +} From 679750a25ee85f5262986b169b2cee9782972f67 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 13 Feb 2024 02:38:50 +0000 Subject: [PATCH 15/19] next update The only complete part of this program is probably the TagDetector subsystem. I still need to set up the camera, find out a way to separate entering Turning Mode as a separate command and find a way to cancel a command within a command. --- src/main/java/frc/robot/Constants.java | 11 ++-- .../java/frc/robot/commands/AlignAmpShot.java | 51 +++++++++++++++++-- .../frc/robot/subsystems/TagDetector.java | 40 ++++++++++++++- 3 files changed, 94 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4cf7d12..27c1b33 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,12 +41,17 @@ public static class AprilTags{ public static final int numThreads=1; public static final float quadDecimate=2; public static final float quadSigma=0; - + //6.5 inches to mm + public static final double normalTagLength=165.1; + //pixel to mm scale factor + public static final double pixelScaler=1; //https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html public static final String family="36h11"; - - //this also stores the april tag numbers of all the major destinations + } + public static class AmpAprilTag{ //remember to actually fill this in with the correct april tags public static final int ampNum=0; + // the ratio of pixels to actual length at optimal distance from the robot to the april tag. + public static final double optimalDistance=10; } } diff --git a/src/main/java/frc/robot/commands/AlignAmpShot.java b/src/main/java/frc/robot/commands/AlignAmpShot.java index 66bca1e..ded8d7a 100644 --- a/src/main/java/frc/robot/commands/AlignAmpShot.java +++ b/src/main/java/frc/robot/commands/AlignAmpShot.java @@ -10,16 +10,18 @@ import frc.robot.subsystems.Motors.TurnMotor; import frc.robot.subsystems.TagDetector; import frc.robot.Constants.AprilTags; +import frc.robot.Constants.AmpAprilTag; -import javax.swing.text.html.HTML.Tag; import edu.wpi.first.apriltag.AprilTag; +import org.opencv.core.Mat; 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.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; /** An example command that uses an example subsystem. */ public class AlignAmpShot extends Command { @@ -52,6 +54,10 @@ public class AlignAmpShot extends Command { public double kDRobotMove; public int tag; + public Mat image; + public boolean turningMode; + public double angleDiff; + public boolean readyToFire; /** @@ -60,6 +66,8 @@ public class AlignAmpShot extends Command { * @param subsystem The subsystem used by this command. */ public AlignAmpShot(Encoders encoders, Motors motors, TagDetector tagDetector) { + //remember to add the camera to this command. + this.encoders=encoders; this.motors=motors; this.tagDetector=tagDetector; @@ -131,12 +139,49 @@ public void initialize() { //temporarily setting the movement controller to 1cm. robotMovementController.setTolerance(0.01); - tag=AprilTags.ampNum; + tag=AmpAprilTag.ampNum; + turningMode=false; + readyToFire=false; } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + //take a picture + if (tagDetector.updateDetection(image, tag)){ + //essntially, if an image is found, then run this code + } + else{ + tab=Shuffleboard.getTab("AmpShot Command"); + tab.add("Error","Image not found"); + } + angleDiff=tagDetector.angleDiff(); + if (angleDiff>Math.PI/360){ + //allowing an error of 1 degrees. + + //enter turning mode + + if(turningMode){ + //turn left slightly to check if the angle Diff is clockwise or anticlockwise. + robotBearingController.setSetpoint(0); + motors.setMoveMotors(robotBearingController.calculate(angleDiff)); + } + } + else{ + + //exit turning mode + if (turningMode==false){ + robotMovementController.setSetpoint(AmpAprilTag.optimalDistance); + motors.setMoveMotors(robotMovementController.calculate(tagDetector.pixelToHeightRatio())); + if (robotMovementController.atSetpoint()){ + readyToFire=true; + } + } + } + if (readyToFire){ + //end the command and schedule the fire command + } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/TagDetector.java b/src/main/java/frc/robot/subsystems/TagDetector.java index 08b93ff..7e8e826 100644 --- a/src/main/java/frc/robot/subsystems/TagDetector.java +++ b/src/main/java/frc/robot/subsystems/TagDetector.java @@ -10,7 +10,6 @@ import org.opencv.core.Mat; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,6 +19,14 @@ public class TagDetector extends SubsystemBase { public final AprilTagDetector detector; public final AprilTagDetector.Config config; public AprilTagDetection detection; + + + public double bottomLeft; + public double bottomRight; + public double topRight; + public double topLeft; + public double observedLength; + public double angleDiff; public TagDetector(AprilTagDetector Detector, AprilTagDetector.Config configuration) { detector=Detector; detector.addFamily(AprilTags.family); @@ -43,7 +50,36 @@ public boolean updateDetection(Mat image, int tagID){ } } return false; + } + public double angleDiff(){ + // this code is incapable of telling if the aprilTag is rotated clockwise or anticlockwise but + // it can calculate roughly how much it should turn. + //these are the X coordinates of the april tag corners. We don't need + //to care about the Y coordinates as we can safely assume that the robot's height + //remains somewhat constant. + bottomLeft=detection.getCornerX(0); + bottomRight=detection.getCornerX(1); + topRight=detection.getCornerX(2); + topLeft=detection.getCornerX(3); + //calculating the average difference in the X coordinates of the corners + observedLength=Math.abs(bottomLeft-bottomRight); + observedLength+=Math.abs(topLeft-topRight); + observedLength=observedLength/2*AprilTags.pixelScaler; + angleDiff=Math.acos(observedLength/AprilTags.normalTagLength); + return angleDiff; + } + public double pixelToHeightRatio(){ + //these are the Y coordinates of the paril tag corners. + //this is because the camera is roughly level with the april tag, so we don't + //have to worry about angle offset.. + bottomLeft=detection.getCornerY(0); + bottomRight=detection.getCornerY(1); + topRight=detection.getCornerY(2); + topLeft=detection.getCornerY(3); + observedLength=Math.abs(bottomLeft-bottomRight); + observedLength+=Math.abs(topLeft-topRight); + observedLength=observedLength/2*AprilTags.pixelScaler; + return observedLength/AprilTags.normalTagLength; } - } From 43c7aef94f0d5a60f06e89165a3e33dfdb70ec41 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 13 Feb 2024 03:01:23 +0000 Subject: [PATCH 16/19] This is the enter turning mode command. --- src/main/java/frc/robot/commands/EnterTurningMode.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/java/frc/robot/commands/EnterTurningMode.java diff --git a/src/main/java/frc/robot/commands/EnterTurningMode.java b/src/main/java/frc/robot/commands/EnterTurningMode.java new file mode 100644 index 0000000..e69de29 From d000cbd924537f5bbfa9f58d3d59976593fea06e Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Thu, 15 Feb 2024 12:44:31 +0000 Subject: [PATCH 17/19] First test build we just need to find the number of pixels from the april tag detected by the camera when the robot is 1 m away. --- src/main/java/frc/robot/Constants.java | 15 +- src/main/java/frc/robot/RobotContainer.java | 13 +- .../frc/robot/commands/AlignWithAprilTag.java | 224 ++++++++++++++++++ .../frc/robot/commands/EnterTurningMode.java | 114 +++++++++ .../{AlignAmpShot.java => MoveToAmp.java} | 123 +++++----- .../java/frc/robot/subsystems/Encoders.java | 39 ++- .../frc/robot/subsystems/TagDetector.java | 28 ++- 7 files changed, 473 insertions(+), 83 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AlignWithAprilTag.java rename src/main/java/frc/robot/commands/{AlignAmpShot.java => MoveToAmp.java} (60%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27c1b33..0adbbe9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,6 +23,11 @@ public static class Ports { public static int kDriveBackRightMove = 7; public static int kDriveBackRightTurn = 8; } + + public static class MotorConstants{ + public static double distancePerRotation=10.0; + public static double movementPerRotation=10.0; + } public static class OperatorConstants { public static final int kDriverControllerPort = 0; @@ -43,15 +48,17 @@ public static class AprilTags{ public static final float quadSigma=0; //6.5 inches to mm public static final double normalTagLength=165.1; - //pixel to mm scale factor - public static final double pixelScaler=1; + //the number of pixels the border of the april tag will have at 1m. + public static final double pixelsAtMeter=400; + //the X coordinate of the center when the april tag is aligned with the camera. + public static final double centerXCoordinate=300; //https://docs.wpilib.org/en/stable/docs/software/vision-processing/apriltag/apriltag-intro.html public static final String family="36h11"; } public static class AmpAprilTag{ //remember to actually fill this in with the correct april tags public static final int ampNum=0; - // the ratio of pixels to actual length at optimal distance from the robot to the april tag. - public static final double optimalDistance=10; + //the optimal distance at which the note can be inserted into the amp in m + public static final double optimalDistance=0.1; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ef0e5d1..1ca38e8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,13 @@ package frc.robot; -import frc.robot.commands.AlignAmpShot; +import frc.robot.commands.AlignWithAprilTag; // here is where you put all your commands and subsystems; import frc.robot.commands.Autos; import frc.robot.commands.Drive; import frc.robot.commands.Snap; +import frc.robot.commands.AlignWithAprilTag; +import frc.robot.commands.MoveToAmp; import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; @@ -16,9 +18,11 @@ import frc.robot.subsystems.TagDetector; import frc.robot.Constants.Ports; import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.MotorConstants; 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.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.apriltag.AprilTagDetector; @@ -65,7 +69,7 @@ public class RobotContainer { 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, 0); + 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); private final TagDetector tagDetector= new TagDetector(detector, configuration); @@ -76,7 +80,8 @@ public class RobotContainer { private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButton); private JoystickButton ampShotButton= new JoystickButton(stick, OperatorConstants.AmpShotButton); - + //remember to add the actual shoot into amp command in here. + private final SequentialCommandGroup autoAmpShot= new SequentialCommandGroup(new AlignWithAprilTag(encoders, motors, tagDetector), new MoveToAmp(encoders, motors, tagDetector)); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -96,7 +101,7 @@ public RobotContainer() { */ private void configureBindings() { snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick)); - ampShotButton.whileTrue(new AlignAmpShot(encoders, motors, tagDetector)); + ampShotButton.whileTrue(autoAmpShot); } private void defaultCommands(){ diff --git a/src/main/java/frc/robot/commands/AlignWithAprilTag.java b/src/main/java/frc/robot/commands/AlignWithAprilTag.java new file mode 100644 index 0000000..4f5496f --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignWithAprilTag.java @@ -0,0 +1,224 @@ +// 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.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +import frc.robot.subsystems.TagDetector; +import frc.robot.Constants.AmpAprilTag; + + +import org.opencv.core.Mat; + +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.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class AlignWithAprilTag extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Encoders encoders; + private final Motors motors; + private final TagDetector tagDetector; + + 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 kPRobotTurn; + public double kIRobotTurn; + public double kDRobotTurn; + + public int tag; + public Mat image; + public double angleDiff; + + public boolean firstIteration; + public boolean foundTurningDirection; + public double previousAngleDiff; + public boolean inverseDirection; + public double orientationTime; + public boolean orientationEnter; + + public boolean endCommand; + public double currentBearing; + + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AlignWithAprilTag(Encoders encoders, Motors motors, TagDetector tagDetector) { + //remember to add the camera to this command. + + this.encoders=encoders; + this.motors=motors; + this.tagDetector=tagDetector; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(encoders,motors,tagDetector); + } + + // 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("kPRobotTurn"); + P=tab.add("Robot P constant",0).getEntry(); + kPRobotTurn=P.getDouble(0); + tab= Shuffleboard.getTab("kIRobotTurn"); + I=tab.add("Robot I constant",0).getEntry(); + kIRobotTurn=I.getDouble(0); + tab= Shuffleboard.getTab("kDRobotTurn"); + D=tab.add("Robot D constant",0).getEntry(); + kDRobotTurn=D.getDouble(0); + + //the setpoint is set in a way that will cause the robot to enter turning mode, + //but it does not necessarily have to be used. + 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/4*3); + + 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/4*5); + + 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/4*7);; + + //the PID controller for the robot itself after turning mode has been achieved + robotBearingController= new PIDController(kPRobotTurn, kIRobotTurn, kDRobotTurn); + robotBearingController.enableContinuousInput(0, 2*Math.PI); + robotBearingController.setTolerance(Math.PI/360); + + + tag=AmpAprilTag.ampNum; + //giving it a dummy value + angleDiff=10; + firstIteration=true; + orientationEnter=true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //take a picture + if (tagDetector.updateDetection(image, tag)==false){ + //essntially, if an image is not found, stop this command + tab=Shuffleboard.getTab("AmpShot Command"); + tab.add("Error","Image not found"); + } + angleDiff=tagDetector.angleDiff(tagDetector.distanceToTag()); + if (firstIteration){ + previousAngleDiff=angleDiff; + firstIteration=false; + } + if (angleDiff>Math.PI/360){ + + //allowing an error of 1 degrees. + if(bearingControllerFrontLeft.atSetpoint() || + bearingControllerFrontRight.atSetpoint() || + bearingControllerBackLeft.atSetpoint() || + bearingControllerBackRight.atSetpoint()){ + + if(foundTurningDirection){ + robotBearingController.setSetpoint(0); + if (inverseDirection){ + motors.setMoveMotors(-1*robotBearingController.calculate(angleDiff)); + } else { + motors.setMoveMotors(robotBearingController.calculate(angleDiff)); + } + } + else{ + //turn right for 0.5s to check if the robot should turn anticlockwise or clockwise. + if (orientationEnter){ + orientationTime=System.currentTimeMillis(); + orientationEnter=false; + } + motors.setMoveMotors(0.5); + if (System.currentTimeMillis()-orientationTime>500){ + //essentially, if this is a second iteration, then check how setting move motors affected + //the angle difference + if (previousAngleDiffMath.PI/360){ - //allowing an error of 1 degrees. - - //enter turning mode - - if(turningMode){ - //turn left slightly to check if the angle Diff is clockwise or anticlockwise. - robotBearingController.setSetpoint(0); - motors.setMoveMotors(robotBearingController.calculate(angleDiff)); + + horizontalOffset=tagDetector.distanceFromCenter(); + verticalOffset=tagDetector.distanceToTag()-AmpAprilTag.optimalDistance; + if (firstAngleIteration){ + //in theory, if the vertical offset is zero, this could cause problems, so I am defualting it to 1 cm. + if (verticalOffset==0) + { + verticalOffset=0.01; } + motorGoal=Math.atan(horizontalOffset/verticalOffset); + bearingControllerFrontLeft.setSetpoint(motorGoal); + bearingControllerFrontRight.setSetpoint(motorGoal); + bearingControllerBackLeft.setSetpoint(motorGoal); + bearingControllerBackRight.setSetpoint(motorGoal); + firstAngleIteration=false; } - else{ - - //exit turning mode - if (turningMode==false){ + if (bearingControllerFrontLeft.atSetpoint() || + bearingControllerFrontRight.atSetpoint() || + bearingControllerBackLeft.atSetpoint() || + bearingControllerBackRight.atSetpoint()){ + if (firstMovementIteration){ robotMovementController.setSetpoint(AmpAprilTag.optimalDistance); - motors.setMoveMotors(robotMovementController.calculate(tagDetector.pixelToHeightRatio())); - if (robotMovementController.atSetpoint()){ - readyToFire=true; - } + firstMovementIteration=false; + } + if (robotMovementController.atSetpoint()){ + endCommand=true; } + //pythagoras + offsetMagnitude=Math.sqrt(Math.pow(horizontalOffset,2)+Math.pow(verticalOffset,2)); + motors.setMoveMotors(robotMovementController.calculate(offsetMagnitude)); } - if (readyToFire){ - //end the command and schedule the fire command + else{ + 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); } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + 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); + motors.setMoveMotors(0); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return endCommand; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index d5a649f..ec0a31c 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -14,6 +14,12 @@ public class Encoders extends SubsystemBase { 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; @@ -24,7 +30,7 @@ public class Encoders extends SubsystemBase { 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) { + 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; @@ -34,6 +40,7 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re this.backRightMove = backRightMove; this.backRightTurn = backRightTurn; + // Reset encoder positions frontLeftMove.setPosition(0); frontRightMove.setPosition(0); @@ -44,10 +51,12 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re backLeftTurn.setPosition(0); backRightTurn.setPosition(0); - frontLeftMove.setPositionConversionFactor(distancePerRotation); - frontRightMove.setPositionConversionFactor(distancePerRotation); - backLeftMove.setPositionConversionFactor(distancePerRotation); - backRightMove.setPositionConversionFactor(distancePerRotation); + //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); @@ -57,21 +66,33 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re 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 - return frontLeftMove.getPosition(); + distanceMoved= frontLeftMove.getPosition()-frontLeftPosition; + frontLeftPosition=frontLeftMove.getPosition(); + return distanceMoved; case 2: //return frontRight distance - return frontRightMove.getPosition(); + distanceMoved= frontRightMove.getPosition()-frontRightPosition; + frontRightPosition=frontRightMove.getPosition(); + return distanceMoved; case 3: //return backLeft distance - return backLeftMove.getPosition(); + distanceMoved= backLeftMove.getPosition()-backLeftPosition; + backLeftPosition=backLeftMove.getPosition(); + return distanceMoved; case 4: //return backRight distance - return backRightMove.getPosition(); + distanceMoved= backRightMove.getPosition()-backRightPosition; + backRightPosition=backRightMove.getPosition(); + return distanceMoved; } //this should never happen, but just in case return 0.0; diff --git a/src/main/java/frc/robot/subsystems/TagDetector.java b/src/main/java/frc/robot/subsystems/TagDetector.java index 7e8e826..4457bf9 100644 --- a/src/main/java/frc/robot/subsystems/TagDetector.java +++ b/src/main/java/frc/robot/subsystems/TagDetector.java @@ -25,6 +25,7 @@ public class TagDetector extends SubsystemBase { public double bottomRight; public double topRight; public double topLeft; + public double centerX; public double observedLength; public double angleDiff; public TagDetector(AprilTagDetector Detector, AprilTagDetector.Config configuration) { @@ -51,7 +52,7 @@ public boolean updateDetection(Mat image, int tagID){ } return false; } - public double angleDiff(){ + public double angleDiff(double currentDistance){ // this code is incapable of telling if the aprilTag is rotated clockwise or anticlockwise but // it can calculate roughly how much it should turn. @@ -65,21 +66,30 @@ public double angleDiff(){ //calculating the average difference in the X coordinates of the corners observedLength=Math.abs(bottomLeft-bottomRight); observedLength+=Math.abs(topLeft-topRight); - observedLength=observedLength/2*AprilTags.pixelScaler; + //the problem is, the number of pixels for any given length changes based on its + //distance from the camera. + observedLength=observedLength/2*AprilTags.pixelsAtMeter*currentDistance; angleDiff=Math.acos(observedLength/AprilTags.normalTagLength); return angleDiff; } - public double pixelToHeightRatio(){ - //these are the Y coordinates of the paril tag corners. - //this is because the camera is roughly level with the april tag, so we don't - //have to worry about angle offset.. + public double distanceToTag(){ + //these are the Y coordinates of the april tag corners. We consider them because we can safely + //assume that they are perpendicular to us. bottomLeft=detection.getCornerY(0); bottomRight=detection.getCornerY(1); topRight=detection.getCornerY(2); topLeft=detection.getCornerY(3); + //calculating the average difference in the Y coordinates of the corners observedLength=Math.abs(bottomLeft-bottomRight); - observedLength+=Math.abs(topLeft-topRight); - observedLength=observedLength/2*AprilTags.pixelScaler; - return observedLength/AprilTags.normalTagLength; + //considering that the number of observed pixels is directly proprtional to distance. + return observedLength/AprilTags.pixelsAtMeter; + } + public double distanceFromCenter(){ + //this returns the distance the robot must traverse in order to get the camera to be centered + //on the april tag + centerX=detection.getCenterX(); + observedLength=centerX-AprilTags.centerXCoordinate; + //when this returns a positive value, the robot must shift left (relative to itself) and vice versa + return observedLength/AprilTags.pixelsAtMeter; } } From e3879d6285fdef7292f74897b0d329dac2062970 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 27 Feb 2024 19:44:49 +0000 Subject: [PATCH 18/19] added constants --- src/main/java/frc/robot/Constants.java | 8 +++++- .../frc/robot/commands/AlignWithAprilTag.java | 28 ++++++------------- .../java/frc/robot/commands/MoveToAmp.java | 27 ++++++------------ 3 files changed, 24 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0adbbe9..3f498b8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,9 +36,15 @@ public static class OperatorConstants { } public static class PIDConstants { - public static final double kDrivetrainP = 1.0; + 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; + public static final double kRobotMoveP = 0.2; + public static final double kRobotMoveI = 0.0; + public static final double kRobotMoveD = 0.0; } public static class AprilTags{ //this stores the settings for the april tag detector config and the families to be detected by the detector diff --git a/src/main/java/frc/robot/commands/AlignWithAprilTag.java b/src/main/java/frc/robot/commands/AlignWithAprilTag.java index 4f5496f..360a89d 100644 --- a/src/main/java/frc/robot/commands/AlignWithAprilTag.java +++ b/src/main/java/frc/robot/commands/AlignWithAprilTag.java @@ -10,6 +10,7 @@ import frc.robot.subsystems.Motors.TurnMotor; import frc.robot.subsystems.TagDetector; import frc.robot.Constants.AmpAprilTag; +import frc.robot.Constants.PIDConstants; import org.opencv.core.Mat; @@ -80,25 +81,14 @@ public AlignWithAprilTag(Encoders encoders, Motors motors, TagDetector tagDetect // 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("kPRobotTurn"); - P=tab.add("Robot P constant",0).getEntry(); - kPRobotTurn=P.getDouble(0); - tab= Shuffleboard.getTab("kIRobotTurn"); - I=tab.add("Robot I constant",0).getEntry(); - kIRobotTurn=I.getDouble(0); - tab= Shuffleboard.getTab("kDRobotTurn"); - D=tab.add("Robot D constant",0).getEntry(); - kDRobotTurn=D.getDouble(0); + kP=PIDConstants.kDrivetrainP; + kI=PIDConstants.kDrivetrainI; + kD=PIDConstants.kDrivetrainD; + + + kPRobotTurn=PIDConstants.kRobotTurnP; + kIRobotTurn=PIDConstants.kRobotTurnI; + kDRobotTurn=PIDConstants.kRobotTurnD; //the setpoint is set in a way that will cause the robot to enter turning mode, //but it does not necessarily have to be used. diff --git a/src/main/java/frc/robot/commands/MoveToAmp.java b/src/main/java/frc/robot/commands/MoveToAmp.java index fd4564a..a0d768a 100644 --- a/src/main/java/frc/robot/commands/MoveToAmp.java +++ b/src/main/java/frc/robot/commands/MoveToAmp.java @@ -10,6 +10,7 @@ import frc.robot.subsystems.Motors.TurnMotor; import frc.robot.subsystems.TagDetector; import frc.robot.Constants.AmpAprilTag; +import frc.robot.Constants.PIDConstants; import org.opencv.core.Mat; @@ -81,25 +82,13 @@ public MoveToAmp(Encoders encoders, Motors motors, TagDetector tagDetector) { // 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("kPRobotMove"); - P=tab.add("Robot P constant",0).getEntry(); - kDRobotMove=P.getDouble(0); - tab= Shuffleboard.getTab("kIRobotMove"); - I=tab.add("Robot I constant",0).getEntry(); - kIRobotMove=I.getDouble(0); - tab= Shuffleboard.getTab("kDRobotMove"); - D=tab.add("Robot D constant",0).getEntry(); - kPRobotMove=D.getDouble(0); + kP=PIDConstants.kDrivetrainP; + kI=PIDConstants.kDrivetrainI; + kD=PIDConstants.kDrivetrainD; + + kPRobotMove=PIDConstants.kRobotMoveP; + kIRobotMove=PIDConstants.kRobotMoveI; + kDRobotMove=PIDConstants.kRobotMoveD; bearingControllerFrontLeft=new PIDController(kP, kI, kD); bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); From ee55f95c37aadbea101adf301a3f7d7933da3b33 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 28 Feb 2024 20:20:31 +0000 Subject: [PATCH 19/19] Updating the encoder code --- src/main/java/frc/robot/RobotContainer.java | 21 +++--- .../frc/robot/commands/AlignWithAprilTag.java | 3 + src/main/java/frc/robot/commands/Drive.java | 6 +- .../java/frc/robot/commands/MoveToAmp.java | 2 + .../java/frc/robot/subsystems/Encoders.java | 74 ++++++++----------- 5 files changed, 52 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ca38e8..553c257 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.apriltag.AprilTagDetector; import edu.wpi.first.apriltag.AprilTagDetector.Config; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -54,14 +57,14 @@ public class RobotContainer { 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 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 AprilTagDetector detector= new AprilTagDetector(); private final AprilTagDetector.Config configuration= new Config(); @@ -69,7 +72,7 @@ public class RobotContainer { 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 Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, MotorConstants.movementPerRotation); private final Gyroscope gyroscope = new Gyroscope(gyro); private final TagDetector tagDetector= new TagDetector(detector, configuration); diff --git a/src/main/java/frc/robot/commands/AlignWithAprilTag.java b/src/main/java/frc/robot/commands/AlignWithAprilTag.java index 360a89d..8155b34 100644 --- a/src/main/java/frc/robot/commands/AlignWithAprilTag.java +++ b/src/main/java/frc/robot/commands/AlignWithAprilTag.java @@ -12,6 +12,7 @@ import frc.robot.Constants.AmpAprilTag; import frc.robot.Constants.PIDConstants; +import javax.swing.text.StyleContext.SmallAttributeSet; import org.opencv.core.Mat; @@ -19,6 +20,7 @@ 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; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. */ @@ -127,6 +129,7 @@ public void initialize() { angleDiff=10; firstIteration=true; orientationEnter=true; + SmartDashboard.putBoolean("Entered align Command", true); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 34827df..60753bf 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -111,13 +111,13 @@ public void execute() { motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.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); } diff --git a/src/main/java/frc/robot/commands/MoveToAmp.java b/src/main/java/frc/robot/commands/MoveToAmp.java index a0d768a..da7ce9a 100644 --- a/src/main/java/frc/robot/commands/MoveToAmp.java +++ b/src/main/java/frc/robot/commands/MoveToAmp.java @@ -19,6 +19,7 @@ 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; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. */ @@ -118,6 +119,7 @@ public void initialize() { tag=AmpAprilTag.ampNum; firstAngleIteration=true; firstMovementIteration=true; + SmartDashboard.putBoolean("Entered move Command", true); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index ec0a31c..4ca81ff 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -1,19 +1,21 @@ 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 RelativeEncoder frontLeftMove; - public RelativeEncoder frontLeftTurn; - public RelativeEncoder frontRightMove; - public RelativeEncoder frontRightTurn; - public RelativeEncoder backLeftMove; - public RelativeEncoder backLeftTurn; - public RelativeEncoder backRightMove; - public RelativeEncoder backRightTurn; + 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; @@ -21,7 +23,6 @@ public class Encoders extends SubsystemBase { 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 @@ -30,7 +31,7 @@ public class Encoders extends SubsystemBase { 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) { + 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; @@ -41,15 +42,7 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re 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 @@ -57,11 +50,6 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re 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; @@ -105,37 +93,37 @@ public double motorTurned(TurnEncoder encoder){ case FRONT_LEFT: //return frontLeft degrees turned distanceRotated = frontLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - frontLeftBearing += deltaRadians; + 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 frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return frontRight degrees turned + distanceRotated = frontRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - frontRightBearing += deltaRadians; + 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 frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return backLeft degrees turned + distanceRotated = backLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - backLeftBearing += deltaRadians; + 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 frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return backRight degrees turned + distanceRotated = backRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - backRightBearing += deltaRadians; + backRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down backRightBearing = backRightBearing%(Math.PI*2); return backRightBearing; } @@ -144,3 +132,5 @@ public double motorTurned(TurnEncoder encoder){ } } + +