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 4b053792502b21f02f4758ed8736641383efb05c Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 12 Feb 2024 13:38:12 +0000 Subject: [PATCH 14/19] Ignore this --- src/main/java/frc/robot/Constants.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7f48304..204806c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,12 +4,18 @@ package frc.robot; + + /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared * globally (i.e. public static). Do not put anything functional in this class. * - *

It is advised to statically import this class (or one of its inner classes) wherever the + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the * constants are needed, to reduce verbosity. */ public final class Constants { From 3004effbc13d0157c590e303bb45e0bf1a4309c2 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 12 Feb 2024 13:39:43 +0000 Subject: [PATCH 15/19] also ignore this --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b69aa72..943d676 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,7 +22,6 @@ 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; From 1f452fd9a4891d9dd2b01b235cad6c8c9e08d7b5 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Thu, 15 Feb 2024 12:50:06 +0000 Subject: [PATCH 16/19] Fixing a logic error I changed it such that the encoder will actually tell you the distance moved since the last time it was called, not the total distance moved as this would be problematic if you were using two different commands during that time. I have also used two different constants, distancePerRotation meaning the circumference of the motor. movementPerRotation meaning the actual distance travelled by the robot when the move motors rotate once. --- src/main/java/frc/robot/Constants.java | 5 +++ src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/subsystems/Encoders.java | 41 +++++++++++++++---- 3 files changed, 39 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 204806c..b1479da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,6 +29,11 @@ public static class Ports { public static int kDriveBackRightMove = 7; public static int kDriveBackRightTurn = 8; } + + public static class MotorConstants{ + public static double distancePerRotation=100; + public static double movementPerRotation=400; + } public static class OperatorConstants { public static final int kDriverControllerPort = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 943d676..8090588 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import frc.robot.subsystems.Encoders; import frc.robot.subsystems.Motors; import frc.robot.Constants.Ports; +import frc.robot.Constants.MotorConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.Gyroscope; import edu.wpi.first.wpilibj.Joystick; @@ -59,7 +60,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); // 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 d5a649f..574382a 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; @@ -123,3 +144,5 @@ public double motorTurned(TurnEncoder encoder){ } } + + From add4851774532c569fdb80eb59c88e33541902d7 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 28 Feb 2024 16:19:13 +0000 Subject: [PATCH 17/19] created the command template --- .../frc/robot/commands/AutoTurnDegrees.java | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AutoTurnDegrees.java diff --git a/src/main/java/frc/robot/commands/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/AutoTurnDegrees.java new file mode 100644 index 0000000..666e925 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoTurnDegrees.java @@ -0,0 +1,80 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import frc.robot.subsystems.Motors; +import frc.robot.subsystems.Motors.TurnMotor; +import frc.robot.subsystems.Encoders; +import frc.robot.subsystems.Encoders.TurnEncoder; +import frc.robot.subsystems.Gyroscope; +import frc.robot.Constants.PIDConstants; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class AutoTurnDegrees extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Motors motors; + private final Encoders encoders; + private final Gyroscope gyro; + private double angleGoal; + + private double kP; + private double kI; + private double kD; + private PIDController frontLeftBearingController; + private PIDController frontRightBearingController; + private PIDController backLeftBearingController; + private PIDController backRightBearingController; + + private double kPRobotTurn; + private double kIRobotTurn; + private double kDRobotTurn; + private PIDController robotBearingController; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AutoTurnDegrees(Motors motors, Encoders encoders, Gyroscope gyro, double angleGoal) { + this.motors=motors; + this.encoders=encoders; + this.gyro=gyro; + this.angleGoal=angleGoal; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(motors,encoders,gyro); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + kP=PIDConstants.kDrivetrainP; + kI=PIDConstants.kDrivetrainI; + kD=PIDConstants.kDrivetrainD; + + kPRobotTurn=PIDConstants.kRobotTurnP; + kIRobotTurn=PIDConstants.kRobotTurnI; + kDRobotTurn=PIDConstants.kRobotTurnD; + + frontLeftBearingController.setP(kP); + frontLeftBearingController.setI(kI); + frontLeftBearingController.setD(kD); + } + + // 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; + } +} From 25285c6523bc72d965da45d5810e6b719edd14c6 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 28 Feb 2024 20:36:18 +0000 Subject: [PATCH 18/19] updated code --- src/main/java/frc/robot/Constants.java | 5 +- .../frc/robot/commands/AutoTurnDegrees.java | 58 ++++++++++++++++++- src/main/java/frc/robot/commands/Drive.java | 8 +-- 3 files changed, 64 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b1479da..4814417 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -40,8 +40,11 @@ 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; } } diff --git a/src/main/java/frc/robot/commands/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/AutoTurnDegrees.java index 666e925..14c67cb 100644 --- a/src/main/java/frc/robot/commands/AutoTurnDegrees.java +++ b/src/main/java/frc/robot/commands/AutoTurnDegrees.java @@ -33,6 +33,9 @@ public class AutoTurnDegrees extends Command { private double kIRobotTurn; private double kDRobotTurn; private PIDController robotBearingController; + private double currentBearing; + + private boolean endCommand; /** * Creates a new ExampleCommand. @@ -62,11 +65,62 @@ public void initialize() { frontLeftBearingController.setP(kP); frontLeftBearingController.setI(kI); frontLeftBearingController.setD(kD); + frontLeftBearingController.enableContinuousInput(0, Math.PI*2); + frontLeftBearingController.setSetpoint(Math.PI/4); + + frontRightBearingController.setP(kP); + frontRightBearingController.setI(kI); + frontRightBearingController.setD(kD); + frontRightBearingController.enableContinuousInput(0, Math.PI*2); + frontRightBearingController.setSetpoint(Math.PI*3/4); + + backLeftBearingController.setP(kP); + backLeftBearingController.setI(kI); + backLeftBearingController.setD(kD); + backLeftBearingController.enableContinuousInput(0, Math.PI*2); + backLeftBearingController.setSetpoint(Math.PI*5/4); + + backRightBearingController.setP(kP); + backRightBearingController.setI(kI); + backRightBearingController.setD(kD); + backRightBearingController.enableContinuousInput(0, Math.PI*2); + backRightBearingController.setSetpoint(Math.PI*7/4); + + robotBearingController.setP(kPRobotTurn); + robotBearingController.setI(kIRobotTurn); + robotBearingController.setD(kDRobotTurn); + robotBearingController.enableContinuousInput(0, Math.PI*2); + robotBearingController.setSetpoint(angleGoal); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (frontLeftBearingController.atSetpoint()&& + frontRightBearingController.atSetpoint()&& + backLeftBearingController.atSetpoint()&& + backRightBearingController.atSetpoint()) + { + motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())); + if(robotBearingController.atSetpoint()){ + endCommand=true; + } + } + else + { + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + motors.setTurnMotors(frontLeftBearingController.calculate(currentBearing), TurnMotor.FRONT_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + motors.setTurnMotors(frontRightBearingController.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + motors.setTurnMotors(backLeftBearingController.calculate(currentBearing), TurnMotor.BACK_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + motors.setTurnMotors(backRightBearingController.calculate(currentBearing), TurnMotor.BACK_RIGHT); + } + } // Called once the command ends or is interrupted. @Override @@ -75,6 +129,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return endCommand; } } diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 34827df..c99258c 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -97,7 +97,7 @@ public void execute() { if(previousBearingGoal!=joystick.getDirectionRadians()) { //updating the new goal if the joystick is moved - previousBearingGoal=joystick.getDirectionRadians()+fieldOrientOffset; + previousBearingGoal=joystick.getDirectionRadians(); bearingControllerFrontLeft.setSetpoint(previousBearingGoal); bearingControllerFrontLeft.reset(); bearingControllerFrontRight.setSetpoint(previousBearingGoal); @@ -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); } From 0df914538c7869c3d7b110dc0a21f110523d5a69 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Thu, 29 Feb 2024 09:08:55 +0000 Subject: [PATCH 19/19] updating the code to actually initialise the PID controller --- .../frc/robot/commands/AutoTurnDegrees.java | 20 +++++-------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoTurnDegrees.java b/src/main/java/frc/robot/commands/AutoTurnDegrees.java index 14c67cb..6f2096f 100644 --- a/src/main/java/frc/robot/commands/AutoTurnDegrees.java +++ b/src/main/java/frc/robot/commands/AutoTurnDegrees.java @@ -62,33 +62,23 @@ public void initialize() { kIRobotTurn=PIDConstants.kRobotTurnI; kDRobotTurn=PIDConstants.kRobotTurnD; - frontLeftBearingController.setP(kP); - frontLeftBearingController.setI(kI); - frontLeftBearingController.setD(kD); + frontLeftBearingController= new PIDController(kP, kI, kD); frontLeftBearingController.enableContinuousInput(0, Math.PI*2); frontLeftBearingController.setSetpoint(Math.PI/4); - frontRightBearingController.setP(kP); - frontRightBearingController.setI(kI); - frontRightBearingController.setD(kD); + frontRightBearingController= new PIDController(kP, kI, kD); frontRightBearingController.enableContinuousInput(0, Math.PI*2); frontRightBearingController.setSetpoint(Math.PI*3/4); - backLeftBearingController.setP(kP); - backLeftBearingController.setI(kI); - backLeftBearingController.setD(kD); + backLeftBearingController= new PIDController(kP, kI, kD); backLeftBearingController.enableContinuousInput(0, Math.PI*2); backLeftBearingController.setSetpoint(Math.PI*5/4); - backRightBearingController.setP(kP); - backRightBearingController.setI(kI); - backRightBearingController.setD(kD); + backRightBearingController= new PIDController(kP, kI, kD); backRightBearingController.enableContinuousInput(0, Math.PI*2); backRightBearingController.setSetpoint(Math.PI*7/4); - robotBearingController.setP(kPRobotTurn); - robotBearingController.setI(kIRobotTurn); - robotBearingController.setD(kDRobotTurn); + robotBearingController= new PIDController(kPRobotTurn, kIRobotTurn, kDRobotTurn); robotBearingController.enableContinuousInput(0, Math.PI*2); robotBearingController.setSetpoint(angleGoal); }