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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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 d7905d9d3ffc81a3ed92aa444173052e93409959 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 7 Feb 2024 16:24:02 +0000 Subject: [PATCH 12/34] added the port numbers to the robot container --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e4048dd..809220f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,6 +15,7 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; + public static final int snapButtonNum=1; } public static class PIDConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 66f41c4..82f0416 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import frc.robot.Constants.OperatorConstants; // here is where you put all your commands and subsystems; import frc.robot.commands.Autos; import frc.robot.commands.Drive; @@ -63,7 +64,7 @@ public class RobotContainer { private Joystick stick = new Joystick(OperatorConstants.kDriverControllerPort); // this is the button on the handle of the joystick - private JoystickButton snapButton = new JoystickButton(stick, 1); + private JoystickButton snapButton = new JoystickButton(stick, OperatorConstants.snapButtonNum); 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 13/34] 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 14/34] 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 d8fda1280032e38f6d64f23fd4d9a4550bd24f2c Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 12 Feb 2024 09:45:17 +0000 Subject: [PATCH 15/34] A quick update Changed it so that the new bearing will only update if the joystick points in a direction that would probably result in a new snap heading. --- src/main/java/frc/robot/commands/Snap.java | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index 4c827c6..122bc07 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -133,17 +133,19 @@ public void execute() { currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); } - else{ + 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){ + + if (Math.abs(joystickBearing-previousSnapGoal)>Math.PI/4){ + // 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; //updating the new goal if the joystick is moved robotBearingController.reset(); robotBearingController.setSetpoint(snapGoal); 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 16/34] 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 17/34] 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 18/34] 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 c2c5e33558d9262776356384812bf068d45c4b85 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sat, 24 Feb 2024 08:10:06 +0000 Subject: [PATCH 19/34] new testing version --- src/main/java/frc/robot/Robot.java | 23 +++------------------ src/main/java/frc/robot/commands/Drive.java | 13 ++++++++++-- 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0fc394..746d859 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,11 +20,7 @@ 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 @@ -94,27 +90,14 @@ 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() { - 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. */ diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 34827df..690a210 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -28,10 +28,13 @@ public class Drive extends Command { public double kI; public double kD; - // public ShuffleboardTab tab; + public ShuffleboardTab tab; // public GenericEntry P; // public GenericEntry I; // public GenericEntry D; + + public GenericEntry joystickBearing; + public GenericEntry joystickMagnitude; private double currentBearing; @@ -94,6 +97,12 @@ public void initialize() { @Override public void execute() { fieldOrientOffset=gyro.getBearing(); + tab=Shuffleboard.getTab("Joystick Input"); + joystickBearing=tab.add("joystick Bearing",0).getEntry(); + joystickBearing.setDouble(joystick.getDirectionRadians()); + tab=Shuffleboard.getTab("Joystick Input"); + joystickMagnitude=tab.add("Joystick Magnitude", 0).getEntry(); + joystickMagnitude.setDouble(joystick.getMagnitude()); if(previousBearingGoal!=joystick.getDirectionRadians()) { //updating the new goal if the joystick is moved @@ -119,7 +128,7 @@ public void execute() { 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); + motors.setMoveMotors(joystick.getMagnitude()*0.1); } // Called once the command ends or is interrupted. From 18eeef68808cead440137580af8b198d6b45c660 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sun, 25 Feb 2024 13:03:38 +0000 Subject: [PATCH 20/34] test config --- src/main/java/frc/robot/Robot.java | 3 ++ src/main/java/frc/robot/RobotContainer.java | 25 +++++++------ src/main/java/frc/robot/commands/Drive.java | 37 ++++++++++--------- .../java/frc/robot/subsystems/Encoders.java | 29 ++++++--------- 4 files changed, 47 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 746d859..da8c694 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -31,6 +32,7 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + SmartDashboard.putBoolean("Initialised",true); } /** @@ -47,6 +49,7 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + SmartDashboard.putBoolean("TelePeriodic Running", true); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8090588..902db7a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,12 +16,15 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.Gyroscope; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; + +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.CANSparkLowLevel.MotorType; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.I2C; @@ -47,14 +50,14 @@ public class RobotContainer { private final CANSparkMax backLeftTurn = new CANSparkMax(Ports.kDriveBackLeftTurn,MotorType.kBrushless); private final CANSparkMax backRightMove= new CANSparkMax(Ports.kDriveBackRightMove,MotorType.kBrushless); private final CANSparkMax backRightTurn= new CANSparkMax(Ports.kDriveBackRightTurn,MotorType.kBrushless); - private final RelativeEncoder frontLeftMoveEncoder = frontLeftMove.getEncoder(); - private final RelativeEncoder frontLeftTurnEncoder = frontLeftTurn.getEncoder(); - private final RelativeEncoder frontRightMoveEncoder = frontRightMove.getEncoder(); - private final RelativeEncoder frontRightTurnEncoder = frontRightTurn.getEncoder(); - private final RelativeEncoder backLeftMoveEncoder = backLeftMove.getEncoder(); - private final RelativeEncoder backLeftTurnEncoder = backLeftTurn.getEncoder(); - private final RelativeEncoder backRightMoveEncoder = backRightMove.getEncoder(); - private final RelativeEncoder backRightTurnEncoder = backRightTurn.getEncoder(); + private final AbsoluteEncoder frontLeftMoveEncoder = frontLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontLeftTurnEncoder = frontLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightMoveEncoder = frontRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightTurnEncoder = frontRightTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftMoveEncoder = backLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftTurnEncoder = backLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightMoveEncoder = backRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightTurnEncoder = backRightTurn.getAbsoluteEncoder(Type.kDutyCycle); private final AHRS gyro = new AHRS(I2C.Port.kMXP); @@ -87,12 +90,10 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - - - snapButton.whileTrue(new Snap(motors, encoders, gyroscope, stick)); } private void defaultCommands(){ + SmartDashboard.putBoolean("Set default command", false); motors.setDefaultCommand(new Drive(motors,encoders,gyroscope,stick)); } /** diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 690a210..7e085a8 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -12,6 +12,7 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** An example command that uses an example subsystem. */ public class Drive extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) @@ -91,22 +92,24 @@ public void initialize() { bearingControllerBackRight.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) bearingControllerBackRight.setTolerance(Math.PI/180); + motors.setMoveMotors(0); + motors.setTurnMotors(0,TurnMotor.FRONT_LEFT); + motors.setTurnMotors(0,TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(0,TurnMotor.BACK_LEFT); + motors.setTurnMotors(0,TurnMotor.BACK_RIGHT); + SmartDashboard.putBoolean("In Command", true); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { fieldOrientOffset=gyro.getBearing(); - tab=Shuffleboard.getTab("Joystick Input"); - joystickBearing=tab.add("joystick Bearing",0).getEntry(); - joystickBearing.setDouble(joystick.getDirectionRadians()); - tab=Shuffleboard.getTab("Joystick Input"); - joystickMagnitude=tab.add("Joystick Magnitude", 0).getEntry(); - joystickMagnitude.setDouble(joystick.getMagnitude()); - if(previousBearingGoal!=joystick.getDirectionRadians()) + SmartDashboard.putNumber("joystick/bearing",joystick.getDirectionRadians()); + SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); + if(previousBearingGoal!=joystick.getDirectionRadians()-fieldOrientOffset) { //updating the new goal if the joystick is moved - previousBearingGoal=joystick.getDirectionRadians()+fieldOrientOffset; + previousBearingGoal=joystick.getDirectionRadians()-fieldOrientOffset; bearingControllerFrontLeft.setSetpoint(previousBearingGoal); bearingControllerFrontLeft.reset(); bearingControllerFrontRight.setSetpoint(previousBearingGoal); @@ -116,18 +119,18 @@ public void execute() { bearingControllerBackRight.setSetpoint(previousBearingGoal); bearingControllerBackRight.reset(); } - currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); + //currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.FRONT_LEFT); - currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + //currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.FRONT_RIGHT); - currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + //currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.BACK_LEFT); - 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 + //currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.BACK_RIGHT); + //setting the speed, but at 0.1 scale to ensure no one dies motors.setMoveMotors(joystick.getMagnitude()*0.1); } diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index 574382a..c0b534f 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -2,18 +2,19 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; public class Encoders extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - public RelativeEncoder frontLeftMove; - public RelativeEncoder frontLeftTurn; - public RelativeEncoder frontRightMove; - public RelativeEncoder frontRightTurn; - public RelativeEncoder backLeftMove; - public RelativeEncoder backLeftTurn; - public RelativeEncoder backRightMove; - public RelativeEncoder backRightTurn; + public AbsoluteEncoder frontLeftMove; + public AbsoluteEncoder frontLeftTurn; + public AbsoluteEncoder frontRightMove; + public AbsoluteEncoder frontRightTurn; + public AbsoluteEncoder backLeftMove; + public AbsoluteEncoder backLeftTurn; + public AbsoluteEncoder backRightMove; + public AbsoluteEncoder backRightTurn; //for calculating the distance moved public double frontLeftPosition; public double frontRightPosition; @@ -30,7 +31,7 @@ public class Encoders extends SubsystemBase { public double backLeftBearing; public double backRightBearing; - public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, RelativeEncoder frontRightTurn, RelativeEncoder frontRightMove, RelativeEncoder backLeftMove, RelativeEncoder backLeftTurn, RelativeEncoder backRightMove, RelativeEncoder backRightTurn, double motorRadius, double distancePerRotation, double movementPerRotation) { + public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, AbsoluteEncoder frontRightTurn, AbsoluteEncoder frontRightMove, AbsoluteEncoder backLeftMove, AbsoluteEncoder backLeftTurn, AbsoluteEncoder backRightMove, AbsoluteEncoder backRightTurn, double motorRadius, double distancePerRotation, double movementPerRotation) { this.frontLeftMove = frontLeftMove; this.frontLeftTurn = frontLeftTurn; this.frontRightTurn = frontRightTurn; @@ -41,15 +42,7 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re this.backRightTurn = backRightTurn; - // Reset encoder positions - frontLeftMove.setPosition(0); - frontRightMove.setPosition(0); - backLeftMove.setPosition(0); - backRightMove.setPosition(0); - frontLeftTurn.setPosition(0); - frontRightTurn.setPosition(0); - backLeftTurn.setPosition(0); - backRightTurn.setPosition(0); + //the movement motors use movement per rotation because 1 rotation for the moveement motor doesn't mean that //the robot moves forward by precisely the circumference of the motor From 13bcbfd519b4350b45b49eafd554078ea4e6eb39 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sun, 25 Feb 2024 16:40:24 +0000 Subject: [PATCH 21/34] Working version --- networktables.json | 1 - simgui-ds.json | 92 ------------------- simgui.json | 10 -- src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/Robot.java | 2 - src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/Drive.java | 47 ++++++---- .../java/frc/robot/subsystems/Encoders.java | 45 ++++----- .../java/frc/robot/subsystems/Motors.java | 3 +- 9 files changed, 53 insertions(+), 152 deletions(-) delete mode 100644 networktables.json delete mode 100644 simgui-ds.json delete mode 100644 simgui.json diff --git a/networktables.json b/networktables.json deleted file mode 100644 index fe51488..0000000 --- a/networktables.json +++ /dev/null @@ -1 +0,0 @@ -[] diff --git a/simgui-ds.json b/simgui-ds.json deleted file mode 100644 index 73cc713..0000000 --- a/simgui-ds.json +++ /dev/null @@ -1,92 +0,0 @@ -{ - "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 deleted file mode 100644 index 5f9d275..0000000 --- a/simgui.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - "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 b1479da..34e5af4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,7 +31,6 @@ public static class Ports { } public static class MotorConstants{ - public static double distancePerRotation=100; public static double movementPerRotation=400; } @@ -40,7 +39,7 @@ 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; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index da8c694..a6bb713 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,7 +32,6 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - SmartDashboard.putBoolean("Initialised",true); } /** @@ -49,7 +48,6 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - SmartDashboard.putBoolean("TelePeriodic Running", true); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 902db7a..0766e6a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,7 @@ public class RobotContainer { private final Motors motors= new Motors(frontLeftMove, frontLeftTurn, frontRightMove, frontRightTurn, backLeftMove, backLeftTurn, backRightMove, backRightTurn); //motor radius is configured in mm and distance per rotation is still unkown - private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, 38.1, MotorConstants.distancePerRotation,MotorConstants.movementPerRotation); + private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, MotorConstants.movementPerRotation); private final Gyroscope gyroscope = new Gyroscope(gyro); // remember to set the joystick port diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 7e085a8..7d355ee 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -34,13 +34,13 @@ public class Drive extends Command { // public GenericEntry I; // public GenericEntry D; - public GenericEntry joystickBearing; - public GenericEntry joystickMagnitude; + private double currentBearing; private double previousBearingGoal; private double fieldOrientOffset; + private double joystickBearing; /** @@ -97,19 +97,23 @@ public void initialize() { motors.setTurnMotors(0,TurnMotor.FRONT_RIGHT); motors.setTurnMotors(0,TurnMotor.BACK_LEFT); motors.setTurnMotors(0,TurnMotor.BACK_RIGHT); - SmartDashboard.putBoolean("In Command", true); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - fieldOrientOffset=gyro.getBearing(); - SmartDashboard.putNumber("joystick/bearing",joystick.getDirectionRadians()); SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); - if(previousBearingGoal!=joystick.getDirectionRadians()-fieldOrientOffset) + //Adding Pi to the joystick output so that it is within the range 0 to 2 pi + joystickBearing=joystick.getDirectionRadians(); + if (joystickBearing<0){ + joystickBearing+=Math.PI*2; + } + SmartDashboard.putNumber("joystick/bearing",joystickBearing); + //I won't update the new bearing unless it is 1 degree different to the old bearing. + if(Math.abs(previousBearingGoal-joystickBearing)>Math.PI/360) { //updating the new goal if the joystick is moved - previousBearingGoal=joystick.getDirectionRadians()-fieldOrientOffset; + previousBearingGoal=joystickBearing; bearingControllerFrontLeft.setSetpoint(previousBearingGoal); bearingControllerFrontLeft.reset(); bearingControllerFrontRight.setSetpoint(previousBearingGoal); @@ -119,19 +123,26 @@ public void execute() { bearingControllerBackRight.setSetpoint(previousBearingGoal); bearingControllerBackRight.reset(); } - //currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); - //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.FRONT_LEFT); + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + SmartDashboard.putNumber("Front left PID output", bearingControllerFrontLeft.calculate(currentBearing)*0.5); + SmartDashboard.putNumber("Front Left Setpoint",bearingControllerFrontLeft.getSetpoint()); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.5, TurnMotor.FRONT_LEFT); + SmartDashboard.putNumber("Front left Bearing", currentBearing); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing)*0.5, TurnMotor.FRONT_RIGHT); + SmartDashboard.putNumber("Front right Bearing", currentBearing); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing)*0.5, TurnMotor.BACK_LEFT); + SmartDashboard.putNumber("Back left Bearing", currentBearing); + + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing)*0.5, TurnMotor.BACK_RIGHT); + SmartDashboard.putNumber("Back right Bearing", currentBearing); - //currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); - //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.FRONT_RIGHT); - - //currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); - //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.BACK_LEFT); - - //currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); - //motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.1, TurnMotor.BACK_RIGHT); //setting the speed, but at 0.1 scale to ensure no one dies - motors.setMoveMotors(joystick.getMagnitude()*0.1); + motors.setMoveMotors(joystick.getMagnitude()*0.25); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index c0b534f..4ca81ff 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.AbsoluteEncoder; @@ -22,7 +23,6 @@ public class Encoders extends SubsystemBase { public double backRightPosition; public double distanceMoved; //for calculating the degrees turned - public double motorRadius; public double distanceRotated; public double deltaRadians; //storing value of current bearing of each wheel @@ -31,7 +31,7 @@ public class Encoders extends SubsystemBase { public double backLeftBearing; public double backRightBearing; - public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, AbsoluteEncoder frontRightTurn, AbsoluteEncoder frontRightMove, AbsoluteEncoder backLeftMove, AbsoluteEncoder backLeftTurn, AbsoluteEncoder backRightMove, AbsoluteEncoder backRightTurn, double motorRadius, double distancePerRotation, double movementPerRotation) { + public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, AbsoluteEncoder frontRightTurn, AbsoluteEncoder frontRightMove, AbsoluteEncoder backLeftMove, AbsoluteEncoder backLeftTurn, AbsoluteEncoder backRightMove, AbsoluteEncoder backRightTurn, double movementPerRotation) { this.frontLeftMove = frontLeftMove; this.frontLeftTurn = frontLeftTurn; this.frontRightTurn = frontRightTurn; @@ -50,11 +50,6 @@ public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, Ab frontRightMove.setPositionConversionFactor(movementPerRotation); backLeftMove.setPositionConversionFactor(movementPerRotation); backRightMove.setPositionConversionFactor(movementPerRotation); - frontLeftTurn.setPositionConversionFactor(distancePerRotation); - frontRightTurn.setPositionConversionFactor(distancePerRotation); - backLeftTurn.setPositionConversionFactor(distancePerRotation); - backRightTurn.setPositionConversionFactor(distancePerRotation); - this.motorRadius = motorRadius; frontLeftBearing = 0; frontRightBearing = 0; backLeftBearing = 0; @@ -98,37 +93,37 @@ public double motorTurned(TurnEncoder encoder){ case FRONT_LEFT: //return frontLeft degrees turned distanceRotated = frontLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - frontLeftBearing += deltaRadians; + frontLeftBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down frontLeftBearing = frontLeftBearing%(Math.PI*2); return frontLeftBearing; case FRONT_RIGHT: - //return frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return frontRight degrees turned + distanceRotated = frontRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - frontRightBearing += deltaRadians; + frontRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down frontRightBearing = frontRightBearing%(Math.PI*2); return frontRightBearing; case BACK_LEFT: - //return frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return backLeft degrees turned + distanceRotated = backLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - backLeftBearing += deltaRadians; + backLeftBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down backLeftBearing = backLeftBearing%(Math.PI*2); return backLeftBearing; case BACK_RIGHT: - //return frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return backRight degrees turned + distanceRotated = backRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - backRightBearing += deltaRadians; + backRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down backRightBearing = backRightBearing%(Math.PI*2); return backRightBearing; } diff --git a/src/main/java/frc/robot/subsystems/Motors.java b/src/main/java/frc/robot/subsystems/Motors.java index 0c02915..64c23c1 100644 --- a/src/main/java/frc/robot/subsystems/Motors.java +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -31,10 +31,11 @@ public Motors(CANSparkMax FrontLeftMove, CANSparkMax FrontLeftTurn, CANSparkMax //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); + //backRightMotor is inverted + backRightMove.set(PIDoutput); } public enum TurnMotor { From 76e01997149da82c3c0f59e65bb548288afaeb26 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 27 Feb 2024 18:31:20 +0000 Subject: [PATCH 22/34] field oriented version --- src/main/java/frc/robot/commands/Drive.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 7d355ee..cd76723 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -105,6 +105,8 @@ public void execute() { SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); //Adding Pi to the joystick output so that it is within the range 0 to 2 pi joystickBearing=joystick.getDirectionRadians(); + fieldOrientOffset=gyro.getBearing(); + joystickBearing-=fieldOrientOffset; if (joystickBearing<0){ joystickBearing+=Math.PI*2; } From c87b4b41ffc5ceeb421d1b7026be4d00a0d1de7c Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 27 Feb 2024 18:32:32 +0000 Subject: [PATCH 23/34] added some extra stuff to SmartDashBoard --- src/main/java/frc/robot/commands/Drive.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index cd76723..42b45d2 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -111,6 +111,7 @@ public void execute() { joystickBearing+=Math.PI*2; } SmartDashboard.putNumber("joystick/bearing",joystickBearing); + SmartDashboard.putNumber("gyro offset", fieldOrientOffset); //I won't update the new bearing unless it is 1 degree different to the old bearing. if(Math.abs(previousBearingGoal-joystickBearing)>Math.PI/360) { @@ -126,8 +127,6 @@ public void execute() { bearingControllerBackRight.reset(); } currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); - SmartDashboard.putNumber("Front left PID output", bearingControllerFrontLeft.calculate(currentBearing)*0.5); - SmartDashboard.putNumber("Front Left Setpoint",bearingControllerFrontLeft.getSetpoint()); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.5, TurnMotor.FRONT_LEFT); SmartDashboard.putNumber("Front left Bearing", currentBearing); From da2a1347b291cbb47d68d2a7bb66dbecefaa2c40 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 27 Feb 2024 19:24:04 +0000 Subject: [PATCH 24/34] Snap Update --- src/main/java/frc/robot/commands/Snap.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index 122bc07..5638e36 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -10,8 +10,11 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import java.lang.Math; + /** An example command that uses an example subsystem. */ public class Snap extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) @@ -110,6 +113,13 @@ public void initialize() { robotBearingController= new PIDController(kPRobot, kIRobot, kDRobot); robotBearingController.enableContinuousInput(0, 2*Math.PI); robotBearingController.setTolerance(Math.PI/360); + + //setting all the motors to zero + motors.setTurnMotors(0, TurnMotor.FRONT_LEFT); + motors.setTurnMotors(0, TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(0, TurnMotor.BACK_LEFT); + motors.setTurnMotors(0, TurnMotor.BACK_RIGHT); + motors.setMoveMotors(0); } // Called every time the scheduler runs while the command is scheduled. @@ -137,13 +147,15 @@ public void execute() { { //robot ready to rotate joystickBearing=stick.getDirectionRadians(); - + //essentially I won't bother to update this unless the bearing difference is more than 45 degrees if (Math.abs(joystickBearing-previousSnapGoal)>Math.PI/4){ // coding the data from 1 to 4 joystickBearing= joystickBearing/Math.PI*2; // rounding to closest integer - int estimate=(int) joystickBearing; + int estimate= (int) Math.round(joystickBearing); joystickBearing=estimate; + //adding this because I got a funny feeling about the fact that it is rounded to a long + SmartDashboard.getNumber("approximation", estimate); //converting back to radians snapGoal=joystickBearing*Math.PI/2; //updating the new goal if the joystick is moved From 95694e89d637059686a7186ea71739f239595f67 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 27 Feb 2024 19:36:05 +0000 Subject: [PATCH 25/34] updated the constants --- src/main/java/frc/robot/Constants.java | 5 +++- src/main/java/frc/robot/commands/Snap.java | 27 +++++++--------------- 2 files changed, 12 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e0167e8..84a1b6b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,8 +41,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 kRobotTurningP=0.2; + public static final double kRobotTurningI=0.0; + public static final double kRobotTurningD=0.0; } } diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index 5638e36..b9b5b29 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.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.math.controller.PIDController; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.networktables.GenericEntry; @@ -64,26 +65,14 @@ public Snap(Motors motors, Encoders encoders, Gyroscope gyro, Joystick stick) { // 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); + kP=PIDConstants.kDrivetrainP; + kI=PIDConstants.kDrivetrainI; + kD=PIDConstants.kDrivetrainD; + kPRobot=PIDConstants.kRobotTurningP; + kIRobot=PIDConstants.kRobotTurningI; + kDRobot=PIDConstants.kRobotTurningD; + // first the turn motors need to get ready for turning bearingControllerFrontLeft=new PIDController(kP, kI, kD); bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); From 151696a2022715bbee1d6e614835de4e45ea3b06 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Wed, 28 Feb 2024 17:04:34 +0000 Subject: [PATCH 26/34] update to encoder code --- src/main/java/frc/robot/RobotContainer.java | 21 +++--- src/main/java/frc/robot/commands/Drive.java | 8 +-- src/main/java/frc/robot/commands/Snap.java | 1 + .../java/frc/robot/subsystems/Encoders.java | 72 ++++++++----------- 4 files changed, 47 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9abe6bd..bd67e73 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -48,20 +51,20 @@ public class RobotContainer { private final CANSparkMax backLeftTurn = new CANSparkMax(Ports.kDriveBackLeftTurn,MotorType.kBrushless); private final CANSparkMax backRightMove= new CANSparkMax(Ports.kDriveBackRightMove,MotorType.kBrushless); private final CANSparkMax backRightTurn= new CANSparkMax(Ports.kDriveBackRightTurn,MotorType.kBrushless); - private final RelativeEncoder frontLeftMoveEncoder = frontLeftMove.getEncoder(); - private final RelativeEncoder frontLeftTurnEncoder = frontLeftTurn.getEncoder(); - private final RelativeEncoder frontRightMoveEncoder = frontRightMove.getEncoder(); - private final RelativeEncoder frontRightTurnEncoder = frontRightTurn.getEncoder(); - private final RelativeEncoder backLeftMoveEncoder = backLeftMove.getEncoder(); - private final RelativeEncoder backLeftTurnEncoder = backLeftTurn.getEncoder(); - private final RelativeEncoder backRightMoveEncoder = backRightMove.getEncoder(); - private final RelativeEncoder backRightTurnEncoder = backRightTurn.getEncoder(); + private final AbsoluteEncoder frontLeftMoveEncoder = frontLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontLeftTurnEncoder = frontLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightMoveEncoder = frontRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder frontRightTurnEncoder = frontRightTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftMoveEncoder = backLeftMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backLeftTurnEncoder = backLeftTurn.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightMoveEncoder = backRightMove.getAbsoluteEncoder(Type.kDutyCycle); + private final AbsoluteEncoder backRightTurnEncoder = backRightTurn.getAbsoluteEncoder(Type.kDutyCycle); private final AHRS gyro = new AHRS(I2C.Port.kMXP); private final Motors motors= new Motors(frontLeftMove, frontLeftTurn, frontRightMove, frontRightTurn, backLeftMove, backLeftTurn, backRightMove, backRightTurn); //motor radius is configured in mm and distance per rotation is still unkown - private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, 38.1, MotorConstants.distancePerRotation,MotorConstants.movementPerRotation); + private final Encoders encoders= new Encoders(frontLeftMoveEncoder, frontLeftTurnEncoder, frontRightTurnEncoder, frontRightMoveEncoder, backLeftMoveEncoder, backLeftTurnEncoder, backRightMoveEncoder, backRightTurnEncoder, MotorConstants.movementPerRotation); private final Gyroscope gyroscope = new Gyroscope(gyro); // remember to set the joystick port diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 34827df..1fc834b 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -108,16 +108,16 @@ public void execute() { bearingControllerBackRight.reset(); } currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing)*0.5, TurnMotor.FRONT_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing)*0.5, TurnMotor.FRONT_RIGHT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing)*0.5, TurnMotor.BACK_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.BACK_RIGHT); + motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing)*0.5, 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 index b9b5b29..e7cf23f 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -109,6 +109,7 @@ public void initialize() { motors.setTurnMotors(0, TurnMotor.BACK_LEFT); motors.setTurnMotors(0, TurnMotor.BACK_RIGHT); motors.setMoveMotors(0); + SmartDashboard.putBoolean("Command active", true); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index 574382a..4ca81ff 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -1,19 +1,21 @@ package frc.robot.subsystems; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; public class Encoders extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - public RelativeEncoder frontLeftMove; - public RelativeEncoder frontLeftTurn; - public RelativeEncoder frontRightMove; - public RelativeEncoder frontRightTurn; - public RelativeEncoder backLeftMove; - public RelativeEncoder backLeftTurn; - public RelativeEncoder backRightMove; - public RelativeEncoder backRightTurn; + public AbsoluteEncoder frontLeftMove; + public AbsoluteEncoder frontLeftTurn; + public AbsoluteEncoder frontRightMove; + public AbsoluteEncoder frontRightTurn; + public AbsoluteEncoder backLeftMove; + public AbsoluteEncoder backLeftTurn; + public AbsoluteEncoder backRightMove; + public AbsoluteEncoder backRightTurn; //for calculating the distance moved public double frontLeftPosition; public double frontRightPosition; @@ -21,7 +23,6 @@ public class Encoders extends SubsystemBase { public double backRightPosition; public double distanceMoved; //for calculating the degrees turned - public double motorRadius; public double distanceRotated; public double deltaRadians; //storing value of current bearing of each wheel @@ -30,7 +31,7 @@ public class Encoders extends SubsystemBase { public double backLeftBearing; public double backRightBearing; - public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, RelativeEncoder frontRightTurn, RelativeEncoder frontRightMove, RelativeEncoder backLeftMove, RelativeEncoder backLeftTurn, RelativeEncoder backRightMove, RelativeEncoder backRightTurn, double motorRadius, double distancePerRotation, double movementPerRotation) { + public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, AbsoluteEncoder frontRightTurn, AbsoluteEncoder frontRightMove, AbsoluteEncoder backLeftMove, AbsoluteEncoder backLeftTurn, AbsoluteEncoder backRightMove, AbsoluteEncoder backRightTurn, double movementPerRotation) { this.frontLeftMove = frontLeftMove; this.frontLeftTurn = frontLeftTurn; this.frontRightTurn = frontRightTurn; @@ -41,15 +42,7 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re this.backRightTurn = backRightTurn; - // Reset encoder positions - frontLeftMove.setPosition(0); - frontRightMove.setPosition(0); - backLeftMove.setPosition(0); - backRightMove.setPosition(0); - frontLeftTurn.setPosition(0); - frontRightTurn.setPosition(0); - backLeftTurn.setPosition(0); - backRightTurn.setPosition(0); + //the movement motors use movement per rotation because 1 rotation for the moveement motor doesn't mean that //the robot moves forward by precisely the circumference of the motor @@ -57,11 +50,6 @@ public Encoders(RelativeEncoder frontLeftMove, RelativeEncoder frontLeftTurn, Re frontRightMove.setPositionConversionFactor(movementPerRotation); backLeftMove.setPositionConversionFactor(movementPerRotation); backRightMove.setPositionConversionFactor(movementPerRotation); - frontLeftTurn.setPositionConversionFactor(distancePerRotation); - frontRightTurn.setPositionConversionFactor(distancePerRotation); - backLeftTurn.setPositionConversionFactor(distancePerRotation); - backRightTurn.setPositionConversionFactor(distancePerRotation); - this.motorRadius = motorRadius; frontLeftBearing = 0; frontRightBearing = 0; backLeftBearing = 0; @@ -105,37 +93,37 @@ public double motorTurned(TurnEncoder encoder){ case FRONT_LEFT: //return frontLeft degrees turned distanceRotated = frontLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - frontLeftBearing += deltaRadians; + frontLeftBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down frontLeftBearing = frontLeftBearing%(Math.PI*2); return frontLeftBearing; case FRONT_RIGHT: - //return frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return frontRight degrees turned + distanceRotated = frontRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - frontRightBearing += deltaRadians; + frontRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down frontRightBearing = frontRightBearing%(Math.PI*2); return frontRightBearing; case BACK_LEFT: - //return frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return backLeft degrees turned + distanceRotated = backLeftTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - backLeftBearing += deltaRadians; + backLeftBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down backLeftBearing = backLeftBearing%(Math.PI*2); return backLeftBearing; case BACK_RIGHT: - //return frontLeft degrees turned - distanceRotated = frontLeftTurn.getPosition(); + //return backRight degrees turned + distanceRotated = backRightTurn.getPosition(); + SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians - deltaRadians = distanceRotated/motorRadius; - //if deltaRadians is more than 2pi, I am resetting it down - backRightBearing += deltaRadians; + backRightBearing = distanceRotated*2*Math.PI; + //if the is more than 2pi, I am resetting it down backRightBearing = backRightBearing%(Math.PI*2); return backRightBearing; } From dd29846a1b8e21371e6123c845bfc7c73410fe11 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 4 Mar 2024 18:29:13 +0000 Subject: [PATCH 27/34] added more smartdashboard stuff --- src/main/java/frc/robot/commands/Snap.java | 1 + src/main/java/frc/robot/subsystems/Encoders.java | 4 ---- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index e7cf23f..7412c78 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -135,6 +135,7 @@ public void execute() { } else { + SmartDashboard.putBoolean("ready to turn",true); //robot ready to rotate joystickBearing=stick.getDirectionRadians(); //essentially I won't bother to update this unless the bearing difference is more than 45 degrees diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index 4ca81ff..9f78d34 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -93,7 +93,6 @@ public double motorTurned(TurnEncoder encoder){ case FRONT_LEFT: //return frontLeft degrees turned distanceRotated = frontLeftTurn.getPosition(); - SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians frontLeftBearing = distanceRotated*2*Math.PI; //if the is more than 2pi, I am resetting it down @@ -102,7 +101,6 @@ public double motorTurned(TurnEncoder encoder){ case FRONT_RIGHT: //return frontRight degrees turned distanceRotated = frontRightTurn.getPosition(); - SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians frontRightBearing = distanceRotated*2*Math.PI; //if the is more than 2pi, I am resetting it down @@ -111,7 +109,6 @@ public double motorTurned(TurnEncoder encoder){ case BACK_LEFT: //return backLeft degrees turned distanceRotated = backLeftTurn.getPosition(); - SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians backLeftBearing = distanceRotated*2*Math.PI; //if the is more than 2pi, I am resetting it down @@ -120,7 +117,6 @@ public double motorTurned(TurnEncoder encoder){ case BACK_RIGHT: //return backRight degrees turned distanceRotated = backRightTurn.getPosition(); - SmartDashboard.putNumber("FrontLeft Rotations",distanceRotated); //using radians backRightBearing = distanceRotated*2*Math.PI; //if the is more than 2pi, I am resetting it down From b58707cca62c7c553eb1f7871e81023fa55ed1bb Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sat, 9 Mar 2024 09:59:09 +0000 Subject: [PATCH 28/34] Testing update --- src/main/java/frc/robot/Constants.java | 8 +++- src/main/java/frc/robot/commands/Snap.java | 44 +++++++++++-------- .../java/frc/robot/subsystems/Gyroscope.java | 3 ++ 3 files changed, 34 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 84a1b6b..a100248 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,11 +41,15 @@ public static class OperatorConstants { } public static class PIDConstants { - public static final double kDrivetrainP = 0.2; + public static final double kDrivetrainP = 0.25; public static final double kDrivetrainI = 0.0; public static final double kDrivetrainD = 0.0; - public static final double kRobotTurningP=0.2; + public static final double kRobotTurningP=0.01; public static final double kRobotTurningI=0.0; public static final double kRobotTurningD=0.0; } + public static class SnapConstants{ + public static final double snapAngleTolerance=2.5; + public static final double snapBearingTolerance=12.5; + } } diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index 7412c78..86c6555 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -6,6 +6,7 @@ import frc.robot.subsystems.Motors.TurnMotor; import frc.robot.subsystems.Gyroscope; import frc.robot.Constants.PIDConstants; +import frc.robot.Constants.SnapConstants; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.networktables.GenericEntry; @@ -31,9 +32,6 @@ public class Snap extends Command { private PIDController robotBearingController; public ShuffleboardTab tab; - public GenericEntry P; - public GenericEntry I; - public GenericEntry D; //for the motors public double kP; public double kI; @@ -76,32 +74,32 @@ public void initialize() { // first the turn motors need to get ready for turning bearingControllerFrontLeft=new PIDController(kP, kI, kD); bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); - //setting tolerance to +=1 degree (radians equivalent) - bearingControllerFrontLeft.setTolerance(Math.PI/360); - bearingControllerFrontLeft.setSetpoint(Math.PI/4); + //setting tolerance to +=2.5 degree (radians equivalent) + bearingControllerFrontLeft.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); + bearingControllerFrontLeft.setSetpoint(Math.PI*7/4); bearingControllerFrontRight=new PIDController(kP, kI, kD); bearingControllerFrontRight.enableContinuousInput(0, 2*Math.PI); - //setting tolerance to +=1 degree (radians equivalent) - bearingControllerFrontRight.setTolerance(Math.PI/360); - bearingControllerFrontRight.setSetpoint(Math.PI*3/4); + //setting tolerance to +=2.5 degree (radians equivalent) + bearingControllerFrontRight.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); + bearingControllerFrontRight.setSetpoint(Math.PI*5/4); bearingControllerBackLeft=new PIDController(kP, kI, kD); bearingControllerBackLeft.enableContinuousInput(0, 2*Math.PI); - //setting tolerance to +=1 degree (radians equivalent) - bearingControllerBackLeft.setTolerance(Math.PI/360); - bearingControllerBackLeft.setSetpoint(Math.PI*5/4); + //setting tolerance to +=2.5 degree (radians equivalent) + bearingControllerBackLeft.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); + bearingControllerBackLeft.setSetpoint(Math.PI*1/4); bearingControllerBackRight=new PIDController(kP, kI, kD); bearingControllerBackRight.enableContinuousInput(0, 2*Math.PI); - //setting tolerance to +=1 degree (radians equivalent) - bearingControllerBackRight.setTolerance(Math.PI/360); + //setting tolerance to +=2.5 degree (radians equivalent) + bearingControllerBackRight.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); 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); + robotBearingController.setTolerance(Math.PI/180*SnapConstants.snapBearingTolerance); //setting all the motors to zero motors.setTurnMotors(0, TurnMotor.FRONT_LEFT); @@ -126,18 +124,25 @@ public void execute() { 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); + + SmartDashboard.putBoolean("FL ready",bearingControllerFrontLeft.atSetpoint()); + SmartDashboard.putBoolean("FR ready", bearingControllerFrontRight.atSetpoint()); + SmartDashboard.putBoolean("BL ready", bearingControllerBackLeft.atSetpoint()); + SmartDashboard.putBoolean("BR ready", bearingControllerBackRight.atSetpoint()); } else { - SmartDashboard.putBoolean("ready to turn",true); - //robot ready to rotate + SmartDashboard.putNumber("previousSnapGoal", previousSnapGoal); joystickBearing=stick.getDirectionRadians(); + //converting the joystickBearing to range 0 to 2pi + joystickBearing+=Math.PI; //essentially I won't bother to update this unless the bearing difference is more than 45 degrees if (Math.abs(joystickBearing-previousSnapGoal)>Math.PI/4){ // coding the data from 1 to 4 @@ -146,7 +151,7 @@ public void execute() { int estimate= (int) Math.round(joystickBearing); joystickBearing=estimate; //adding this because I got a funny feeling about the fact that it is rounded to a long - SmartDashboard.getNumber("approximation", estimate); + SmartDashboard.putNumber("approximation", estimate); //converting back to radians snapGoal=joystickBearing*Math.PI/2; //updating the new goal if the joystick is moved @@ -154,6 +159,7 @@ public void execute() { robotBearingController.setSetpoint(snapGoal); previousSnapGoal=snapGoal; } + SmartDashboard.putNumber("current Bearing", gyro.getBearing()); motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())); } } diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java index 1841aa8..8e667ea 100644 --- a/src/main/java/frc/robot/subsystems/Gyroscope.java +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -15,6 +15,9 @@ public Gyroscope(AHRS Gyro){ public double getBearing(){ //returns bearing in radians convertedBearing= gyro.getAngle()%360; + if (convertedBearing<0){ + convertedBearing+=360; + } convertedBearing=convertedBearing/180*Math.PI; return convertedBearing; } From 786215a340c2860ce2fd37963b3327ddbf82bf7d Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sat, 9 Mar 2024 10:07:09 +0000 Subject: [PATCH 29/34] fixing bugs I noticed while testing snap --- src/main/java/frc/robot/commands/Drive.java | 2 +- src/main/java/frc/robot/subsystems/Gyroscope.java | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 42b45d2..6b9792b 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -104,7 +104,7 @@ public void initialize() { public void execute() { SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); //Adding Pi to the joystick output so that it is within the range 0 to 2 pi - joystickBearing=joystick.getDirectionRadians(); + joystickBearing=joystick.getDirectionRadians()+Math.PI; fieldOrientOffset=gyro.getBearing(); joystickBearing-=fieldOrientOffset; if (joystickBearing<0){ diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java index 1841aa8..8e667ea 100644 --- a/src/main/java/frc/robot/subsystems/Gyroscope.java +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -15,6 +15,9 @@ public Gyroscope(AHRS Gyro){ public double getBearing(){ //returns bearing in radians convertedBearing= gyro.getAngle()%360; + if (convertedBearing<0){ + convertedBearing+=360; + } convertedBearing=convertedBearing/180*Math.PI; return convertedBearing; } From fce08bbdfeb88ad86c7c67e03e4cd555d868c2f6 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Sun, 10 Mar 2024 16:15:33 +0000 Subject: [PATCH 30/34] old version reload this to use the robotBearingController that has a continuous input --- src/main/java/frc/robot/Constants.java | 7 ++++-- src/main/java/frc/robot/commands/Snap.java | 25 ++++++++++++++++------ 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a100248..c05969b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,12 +41,15 @@ public static class OperatorConstants { } public static class PIDConstants { - public static final double kDrivetrainP = 0.25; + public static final double kDrivetrainP = 0.1; public static final double kDrivetrainI = 0.0; public static final double kDrivetrainD = 0.0; - public static final double kRobotTurningP=0.01; + public static final double kRobotTurningP=0.025; public static final double kRobotTurningI=0.0; public static final double kRobotTurningD=0.0; + public static final double snapMotorP = 0.2; + public static final double snapMotorI = 0.1; + public static final double snapMotorD = 0.0; } public static class SnapConstants{ public static final double snapAngleTolerance=2.5; diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index 86c6555..8a26f8e 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -63,9 +63,9 @@ public Snap(Motors motors, Encoders encoders, Gyroscope gyro, Joystick stick) { // Called when the command is initially scheduled. @Override public void initialize() { - kP=PIDConstants.kDrivetrainP; - kI=PIDConstants.kDrivetrainI; - kD=PIDConstants.kDrivetrainD; + kP=PIDConstants.snapMotorP; + kI=PIDConstants.snapMotorI; + kD=PIDConstants.snapMotorD; kPRobot=PIDConstants.kRobotTurningP; kIRobot=PIDConstants.kRobotTurningI; @@ -120,16 +120,20 @@ public void execute() { bearingControllerBackLeft.atSetpoint()==false || bearingControllerBackRight.atSetpoint()==false){ currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + SmartDashboard.putNumber("FL Bearing", currentBearing); motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + SmartDashboard.putNumber("FR Bearing", currentBearing); motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + SmartDashboard.putNumber("BL Bearing", currentBearing); motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + SmartDashboard.putNumber("BR Bearing", currentBearing); motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); SmartDashboard.putBoolean("FL ready",bearingControllerFrontLeft.atSetpoint()); @@ -139,16 +143,22 @@ public void execute() { } else { - SmartDashboard.putNumber("previousSnapGoal", previousSnapGoal); joystickBearing=stick.getDirectionRadians(); + SmartDashboard.putNumber("JoystickBearing", joystickBearing); //converting the joystickBearing to range 0 to 2pi - joystickBearing+=Math.PI; + if(joystickBearing<0){ + joystickBearing+=2*Math.PI; + } //essentially I won't bother to update this unless the bearing difference is more than 45 degrees if (Math.abs(joystickBearing-previousSnapGoal)>Math.PI/4){ // coding the data from 1 to 4 joystickBearing= joystickBearing/Math.PI*2; // rounding to closest integer int estimate= (int) Math.round(joystickBearing); + if (estimate==4){ + //the PIDController doesn't appreciate snapping between 0 and 4, which are essentially the same thing + estimate=0; + } joystickBearing=estimate; //adding this because I got a funny feeling about the fact that it is rounded to a long SmartDashboard.putNumber("approximation", estimate); @@ -160,12 +170,15 @@ public void execute() { previousSnapGoal=snapGoal; } SmartDashboard.putNumber("current Bearing", gyro.getBearing()); - motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())); + SmartDashboard.putNumber("PID setpoint", robotBearingController.getSetpoint()); + SmartDashboard.putNumber("PID output", robotBearingController.calculate(gyro.getBearing())); + motors.setMoveMotors(robotBearingController.calculate(gyro.getBearing())*0.5); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + SmartDashboard.putBoolean("Command active", false); motors.setTurnMotors(0, TurnMotor.FRONT_LEFT); motors.setTurnMotors(0,TurnMotor.FRONT_RIGHT); motors.setTurnMotors(0, TurnMotor.BACK_LEFT); From 0391eff6b4b8fe15576f13248d2aee512bd498b4 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:01:10 +0000 Subject: [PATCH 31/34] Working version --- src/main/java/frc/robot/Constants.java | 8 +-- src/main/java/frc/robot/Robot.java | 37 +++++++------- src/main/java/frc/robot/commands/Snap.java | 59 +++++++++++----------- 3 files changed, 50 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c05969b..6bf3b86 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,15 +44,15 @@ public static class PIDConstants { public static final double kDrivetrainP = 0.1; public static final double kDrivetrainI = 0.0; public static final double kDrivetrainD = 0.0; - public static final double kRobotTurningP=0.025; + public static final double kRobotTurningP=0.15; public static final double kRobotTurningI=0.0; public static final double kRobotTurningD=0.0; public static final double snapMotorP = 0.2; - public static final double snapMotorI = 0.1; + public static final double snapMotorI = 0.2; public static final double snapMotorD = 0.0; } public static class SnapConstants{ - public static final double snapAngleTolerance=2.5; - public static final double snapBearingTolerance=12.5; + public static final double snapAngleTolerance=1.0; + public static final double snapBearingTolerance=2.0; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0fc394..d302b40 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,9 +7,14 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -20,11 +25,9 @@ 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; + private CANSparkMax motor; + private long startTime; + private long currentTime; /** * This function is run when the robot is first started up and should be used for any @@ -88,32 +91,26 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + + } @Override public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); - 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); + motor = new CANSparkMax(10, MotorType.kBrushless); + startTime=System.currentTimeMillis(); } /** This function is called periodically during test mode. */ @Override public void testPeriodic() { - if (frontLeftMove.get()==100){ - FrontLeftMove.set(0); - FrontRightMove.set(0); - BackLeftMove.set(0); - BackRightMove.set(0); + currentTime=System.currentTimeMillis()-startTime; + if (currentTime<2000){ + SmartDashboard.putBoolean("running", true); + motor.set(0.1); } } diff --git a/src/main/java/frc/robot/commands/Snap.java b/src/main/java/frc/robot/commands/Snap.java index 8a26f8e..685c19e 100644 --- a/src/main/java/frc/robot/commands/Snap.java +++ b/src/main/java/frc/robot/commands/Snap.java @@ -76,25 +76,25 @@ public void initialize() { bearingControllerFrontLeft.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=2.5 degree (radians equivalent) bearingControllerFrontLeft.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); - bearingControllerFrontLeft.setSetpoint(Math.PI*7/4); + bearingControllerFrontLeft.setSetpoint(Math.PI*3/4); bearingControllerFrontRight=new PIDController(kP, kI, kD); bearingControllerFrontRight.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=2.5 degree (radians equivalent) bearingControllerFrontRight.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); - bearingControllerFrontRight.setSetpoint(Math.PI*5/4); + bearingControllerFrontRight.setSetpoint(Math.PI*1/4); bearingControllerBackLeft=new PIDController(kP, kI, kD); bearingControllerBackLeft.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=2.5 degree (radians equivalent) bearingControllerBackLeft.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); - bearingControllerBackLeft.setSetpoint(Math.PI*1/4); + bearingControllerBackLeft.setSetpoint(Math.PI*5/4); bearingControllerBackRight=new PIDController(kP, kI, kD); bearingControllerBackRight.enableContinuousInput(0, 2*Math.PI); //setting tolerance to +=2.5 degree (radians equivalent) bearingControllerBackRight.setTolerance(Math.PI/180*SnapConstants.snapAngleTolerance); - bearingControllerBackRight.setSetpoint(Math.PI*7/4); + bearingControllerBackRight.setSetpoint(Math.PI*3/4); //the PID controller for the robot itself after turning mode has been achieved robotBearingController= new PIDController(kPRobot, kIRobot, kDRobot); @@ -113,36 +113,35 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //if any of the motors are not ready to turn, this code won't run - if ( - bearingControllerFrontLeft.atSetpoint()==false || - bearingControllerFrontRight.atSetpoint()==false || - bearingControllerBackLeft.atSetpoint()==false || - bearingControllerBackRight.atSetpoint()==false){ - currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); - SmartDashboard.putNumber("FL Bearing", currentBearing); - motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); - currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); - SmartDashboard.putNumber("FR Bearing", currentBearing); - motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_LEFT); + SmartDashboard.putNumber("FL Bearing", currentBearing); + motors.setTurnMotors(bearingControllerFrontLeft.calculate(currentBearing), TurnMotor.FRONT_LEFT); + + currentBearing=encoders.motorTurned(TurnEncoder.FRONT_RIGHT); + SmartDashboard.putNumber("FR Bearing", currentBearing); + motors.setTurnMotors(bearingControllerFrontRight.calculate(currentBearing), TurnMotor.FRONT_RIGHT); - currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); - SmartDashboard.putNumber("BL Bearing", currentBearing); - motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); + currentBearing=encoders.motorTurned(TurnEncoder.BACK_LEFT); + SmartDashboard.putNumber("BL Bearing", currentBearing); + motors.setTurnMotors(bearingControllerBackLeft.calculate(currentBearing), TurnMotor.BACK_LEFT); - currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); - SmartDashboard.putNumber("BR Bearing", currentBearing); - motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); - - SmartDashboard.putBoolean("FL ready",bearingControllerFrontLeft.atSetpoint()); - SmartDashboard.putBoolean("FR ready", bearingControllerFrontRight.atSetpoint()); - SmartDashboard.putBoolean("BL ready", bearingControllerBackLeft.atSetpoint()); - SmartDashboard.putBoolean("BR ready", bearingControllerBackRight.atSetpoint()); - } - else - { + currentBearing=encoders.motorTurned(TurnEncoder.BACK_RIGHT); + SmartDashboard.putNumber("BR Bearing", currentBearing); + motors.setTurnMotors(bearingControllerBackRight.calculate(currentBearing), TurnMotor.BACK_RIGHT); + + SmartDashboard.putBoolean("FL ready",bearingControllerFrontLeft.atSetpoint()); + SmartDashboard.putBoolean("FR ready", bearingControllerFrontRight.atSetpoint()); + SmartDashboard.putBoolean("BL ready", bearingControllerBackLeft.atSetpoint()); + SmartDashboard.putBoolean("BR ready", bearingControllerBackRight.atSetpoint()); + + //if any of the motors are not ready to turn, this code won't run + if ( + bearingControllerFrontLeft.atSetpoint() || + bearingControllerFrontRight.atSetpoint() || + bearingControllerBackLeft.atSetpoint() || + bearingControllerBackRight.atSetpoint()){ joystickBearing=stick.getDirectionRadians(); SmartDashboard.putNumber("JoystickBearing", joystickBearing); //converting the joystickBearing to range 0 to 2pi From 2518cea60808409b28531509f404731f3360cee7 Mon Sep 17 00:00:00 2001 From: spamtactics <108055980+spamtactics@users.noreply.github.com> Date: Tue, 12 Mar 2024 20:44:07 +0000 Subject: [PATCH 32/34] Another quick update to errors I noticed while testing snap --- src/main/java/frc/robot/commands/Drive.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 6b9792b..440250e 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -104,10 +104,15 @@ public void initialize() { public void execute() { SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); //Adding Pi to the joystick output so that it is within the range 0 to 2 pi - joystickBearing=joystick.getDirectionRadians()+Math.PI; + joystickBearing=joystick.getDirectionRadians(); + if (joystickBearing<0){ + //converting joystick bearing from range 0 to 2pi + joystickBearing+=Math.PI*2; + } fieldOrientOffset=gyro.getBearing(); joystickBearing-=fieldOrientOffset; if (joystickBearing<0){ + //converting the updated joystick bearing from range 0 to 2pi joystickBearing+=Math.PI*2; } SmartDashboard.putNumber("joystick/bearing",joystickBearing); @@ -123,7 +128,16 @@ public void execute() { bearingControllerFrontRight.reset(); bearingControllerBackLeft.setSetpoint(previousBearingGoal); bearingControllerBackLeft.reset(); - bearingControllerBackRight.setSetpoint(previousBearingGoal); + //due to the tendency for the backright motor to be inversed, I will always set the bearing goal to be + //the current bearing + 180 degrees. + if (previousBearingGoal Date: Wed, 13 Mar 2024 16:09:58 +0000 Subject: [PATCH 33/34] Updating drive.java to the working version --- src/main/java/frc/robot/commands/Drive.java | 54 ++++++++++++++++++--- 1 file changed, 46 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 1fc834b..440250e 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -12,6 +12,7 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** An example command that uses an example subsystem. */ public class Drive extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) @@ -28,15 +29,18 @@ public class Drive extends Command { public double kI; public double kD; - // public ShuffleboardTab tab; + public ShuffleboardTab tab; // public GenericEntry P; // public GenericEntry I; // public GenericEntry D; + + private double currentBearing; private double previousBearingGoal; private double fieldOrientOffset; + private double joystickBearing; /** @@ -88,38 +92,72 @@ public void initialize() { bearingControllerBackRight.enableContinuousInput(0, Math.PI*2); //setting tolerance to +=1 degree (radians equivalent) bearingControllerBackRight.setTolerance(Math.PI/180); + motors.setMoveMotors(0); + motors.setTurnMotors(0,TurnMotor.FRONT_LEFT); + motors.setTurnMotors(0,TurnMotor.FRONT_RIGHT); + motors.setTurnMotors(0,TurnMotor.BACK_LEFT); + motors.setTurnMotors(0,TurnMotor.BACK_RIGHT); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + SmartDashboard.putNumber("joystick Magnitude", joystick.getMagnitude()); + //Adding Pi to the joystick output so that it is within the range 0 to 2 pi + joystickBearing=joystick.getDirectionRadians(); + if (joystickBearing<0){ + //converting joystick bearing from range 0 to 2pi + joystickBearing+=Math.PI*2; + } fieldOrientOffset=gyro.getBearing(); - if(previousBearingGoal!=joystick.getDirectionRadians()) + joystickBearing-=fieldOrientOffset; + if (joystickBearing<0){ + //converting the updated joystick bearing from range 0 to 2pi + joystickBearing+=Math.PI*2; + } + SmartDashboard.putNumber("joystick/bearing",joystickBearing); + SmartDashboard.putNumber("gyro offset", fieldOrientOffset); + //I won't update the new bearing unless it is 1 degree different to the old bearing. + if(Math.abs(previousBearingGoal-joystickBearing)>Math.PI/360) { //updating the new goal if the joystick is moved - previousBearingGoal=joystick.getDirectionRadians()+fieldOrientOffset; + previousBearingGoal=joystickBearing; bearingControllerFrontLeft.setSetpoint(previousBearingGoal); bearingControllerFrontLeft.reset(); bearingControllerFrontRight.setSetpoint(previousBearingGoal); bearingControllerFrontRight.reset(); bearingControllerBackLeft.setSetpoint(previousBearingGoal); bearingControllerBackLeft.reset(); - bearingControllerBackRight.setSetpoint(previousBearingGoal); + //due to the tendency for the backright motor to be inversed, I will always set the bearing goal to be + //the current bearing + 180 degrees. + if (previousBearingGoal Date: Wed, 13 Mar 2024 16:51:47 +0000 Subject: [PATCH 34/34] added javadocs for readability --- src/main/java/frc/robot/commands/Drive.java | 8 ++++-- .../java/frc/robot/subsystems/Encoders.java | 27 ++++++++++++++++++- .../java/frc/robot/subsystems/Gyroscope.java | 8 ++++++ .../java/frc/robot/subsystems/Motors.java | 25 +++++++++++++++-- 4 files changed, 63 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 440250e..3d7ceb5 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -44,9 +44,12 @@ public class Drive extends Command { /** - * Creates a new ExampleCommand. + * Creates a new DriveCommand. * - * @param subsystem The subsystem used by this command. + * @param motors The motors subsystem used by this command. + * @param encoders The encoders subsystem used by this command. + * @param gyro The gyroscope subsystem used by this command + * @param stick The Joystick used by the driver */ public Drive(Motors motors, Encoders encoders, Gyroscope gyro, Joystick stick) { this.encoders=encoders; @@ -99,6 +102,7 @@ public void initialize() { motors.setTurnMotors(0,TurnMotor.BACK_RIGHT); } + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { diff --git a/src/main/java/frc/robot/subsystems/Encoders.java b/src/main/java/frc/robot/subsystems/Encoders.java index 4ca81ff..8353f32 100644 --- a/src/main/java/frc/robot/subsystems/Encoders.java +++ b/src/main/java/frc/robot/subsystems/Encoders.java @@ -30,7 +30,21 @@ public class Encoders extends SubsystemBase { public double frontRightBearing; public double backLeftBearing; public double backRightBearing; - + /** + * Creates an encoder subsystem to manage the encoders of the robot + * This only works for swerve drives. If you use a different set of encoders for the swerve drive + * you only need to modify the class of the encoder parameters + * + * @param frontLeftMove the encoder of the Front Left Movement Encoder + * @param frontLeftTurn the encoder of the Front Left Turning Encoder + * @param frontRightTurn the encoder of the Front Right Turning Encoder + * @param frontRightMove the encoder of the Front Right Movement Encoder + * @param backLeftMove the encoder of the Back Left Movement Encoder + * @param backLeftTurn the encoder of the Back Left Turning Encoder + * @param backRightMove the encoder of he Back Right Movement Encoder + * @param backRightTurn the encoder of the Back Right Turning Encoder + * @param movementPerRotation the distance that the robot moves for every rotation of the Movement Encoder + */ public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, AbsoluteEncoder frontRightTurn, AbsoluteEncoder frontRightMove, AbsoluteEncoder backLeftMove, AbsoluteEncoder backLeftTurn, AbsoluteEncoder backRightMove, AbsoluteEncoder backRightTurn, double movementPerRotation) { this.frontLeftMove = frontLeftMove; this.frontLeftTurn = frontLeftTurn; @@ -59,6 +73,11 @@ public Encoders(AbsoluteEncoder frontLeftMove, AbsoluteEncoder frontLeftTurn, Ab backLeftPosition=0; backRightPosition=0; } + /** + * This gets the distanceMoved by each since the last time this function was called + * @param motorNum the number of the motor you want to check + * @return the distance Moved since the last time this function was called + */ public double getDistanceMoved(int motorNum){ switch (motorNum){ case 1: @@ -88,6 +107,12 @@ public double getDistanceMoved(int motorNum){ public enum TurnEncoder{ FRONT_LEFT,FRONT_RIGHT, BACK_LEFT, BACK_RIGHT } + /** + * This returns the currrent bearing of any one of the Turn Encoders in radians + * The range is from 0 to 2 PI radians + * @param encoder the TurnEncoder you would like to check + * @return the bearing of the encoder from 0 to 2PI Radians + */ public double motorTurned(TurnEncoder encoder){ switch (encoder){ case FRONT_LEFT: diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java index 8e667ea..141b100 100644 --- a/src/main/java/frc/robot/subsystems/Gyroscope.java +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -9,9 +9,17 @@ public class Gyroscope extends SubsystemBase { public AHRS gyro; public double convertedBearing; + /** + * This creates the Gyroscope subsystem to manage the bearing of the robot + * @param Gyro the AHRS object representing the gyro + */ public Gyroscope(AHRS Gyro){ gyro=Gyro; } + /** + * This returns the bearing of the robot + * @return Returns the bearing of the robot from range 0 to 2PI radians. + */ public double getBearing(){ //returns bearing in radians convertedBearing= gyro.getAngle()%360; diff --git a/src/main/java/frc/robot/subsystems/Motors.java b/src/main/java/frc/robot/subsystems/Motors.java index 64c23c1..b8fc2d2 100644 --- a/src/main/java/frc/robot/subsystems/Motors.java +++ b/src/main/java/frc/robot/subsystems/Motors.java @@ -17,7 +17,18 @@ public class Motors extends SubsystemBase { public static CANSparkMax backRightMove; public static CANSparkMax backRightTurn; - + /** + * This creates the Motors subsystem to manage what power the motors are set at. + * This only works for a swerve drive. + * @param FrontLeftMove the Front Left Movement Motor + * @param FrontLeftTurn the Front Left Turning Motor + * @param FrontRightMove the Front Right Movement Motor + * @param FrontRightTurn the Front Right Turning Motor + * @param BackLeftMove the Back Left Movement Motor + * @param BackLeftTurn the Back Left Turning Motor + * @param BackRightMove the Back Right Movement Motor + * @param BackRightTurn the Back Right Turning Motor + */ public Motors(CANSparkMax FrontLeftMove, CANSparkMax FrontLeftTurn, CANSparkMax FrontRightMove, CANSparkMax FrontRightTurn, CANSparkMax BackLeftMove, CANSparkMax BackLeftTurn, CANSparkMax BackRightMove, CANSparkMax BackRightTurn) { frontLeftMove=FrontLeftMove; frontLeftTurn=FrontLeftTurn; @@ -32,6 +43,11 @@ public Motors(CANSparkMax FrontLeftMove, CANSparkMax FrontLeftTurn, CANSparkMax frontRightMove.follow(FrontLeftMove); backLeftMove.follow(FrontLeftMove); } + /** + * This sets the move motors to go at a certain power + * @param PIDoutput The power that you want to set the motors at, ideally controller by a + * PID controller. Only takes from range -1 to 1. + */ public void setMoveMotors(double PIDoutput){ frontLeftMove.set(PIDoutput); //backRightMotor is inverted @@ -41,7 +57,12 @@ public void setMoveMotors(double PIDoutput){ public enum TurnMotor { FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT } - + /** + * This sets the turn motors to go at a certain power + * @param PIDOutput The power that you want to set the motors at, ideally controller by a + * PID controller. Only takes from range -1 to 1. + * @param motor The Turn motor which you would like to set. + */ public void setTurnMotors(double PIDOutput, TurnMotor motor){ switch (motor) {