From b6f066ccf34d0b94160db5821f92af075745e536 Mon Sep 17 00:00:00 2001 From: AppleMacOS <63877394+AppleMacOS@users.noreply.github.com> Date: Tue, 6 Feb 2024 18:05:45 -0500 Subject: [PATCH] Added the necessary code for paths --- .DataLogTool/datalogtool.json | 5 ++ .SysId/sysid.json | 1 + src/main/java/frc/robot/Constants.java | 32 +++++++++++ src/main/java/frc/robot/Robot.java | 16 ++++++ .../commands/SetShooterAngleCommand.java | 1 - .../java/frc/robot/subsystems/Drivetrain.java | 56 +++++++++---------- .../java/frc/robot/subsystems/Intake.java | 4 +- .../java/frc/robot/subsystems/Limelight.java | 1 - 8 files changed, 82 insertions(+), 34 deletions(-) create mode 100644 .DataLogTool/datalogtool.json create mode 100644 .SysId/sysid.json diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..2300675 --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1,5 @@ +{ + "download": { + "serverTeam": "2626" + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4794dab..ebed588 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,8 @@ package frc.robot; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; + /** * 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 @@ -55,4 +57,34 @@ public static class DIGITAL { public final int INTAKE_LIMIT_SWITCH_OUT = 6; public final int INTAKE_LIMIT_SWITCH_IN = 7; } + + /* + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + * TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO + */ + public static class DriveConstants { + public static final double ksVolts = 0.22; + public static final double kvVoltSecondsPerMeter = 1.98; + public static final double kaVoltSecondsSquaredPerMeter = 0.2; + + public static final double kPDriveVel = 8.5; + + public static final double kTrackwidthMeters = 0.58; + public static final DifferentialDriveKinematics kDriveKinematics = + new DifferentialDriveKinematics(kTrackwidthMeters); + + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 1; + + public static final double kRamseteB = 2; + public static final double kRamseteZeta = 0.7; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fc71385..3e16a29 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,11 +4,17 @@ package frc.robot; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryUtil; import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import java.io.IOException; +import java.nio.file.Path; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -23,6 +29,9 @@ public class Robot extends TimedRobot { public Compressor compressor; + String trajectoryJSON = "paths/YourPath.wpilib.json"; + Trajectory trajectory = new Trajectory(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -33,6 +42,13 @@ public void robotInit() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); compressor = new Compressor(1, PneumaticsModuleType.CTREPCM); + + try { + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + } catch (IOException ex) { + DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace()); + } } /** diff --git a/src/main/java/frc/robot/commands/SetShooterAngleCommand.java b/src/main/java/frc/robot/commands/SetShooterAngleCommand.java index 319ba05..13d7f06 100644 --- a/src/main/java/frc/robot/commands/SetShooterAngleCommand.java +++ b/src/main/java/frc/robot/commands/SetShooterAngleCommand.java @@ -16,7 +16,6 @@ public class SetShooterAngleCommand extends PIDCommand { /** Creates a new SetShooterAngleCommand. */ private AngleShooter angleShooter; - Limelight limelight; public SetShooterAngleCommand(AngleShooter angleShooter, Limelight limelight) { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5ac66da..5f18c62 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,11 +4,10 @@ package frc.robot.subsystems; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.ReplanningConfig; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; @@ -17,7 +16,6 @@ import edu.wpi.first.wpilibj.ADIS16470_IMU; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.drive.MecanumDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -41,15 +39,13 @@ public class Drivetrain extends SubsystemBase { public boolean isTankDrive; public static final ADIS16470_IMU gyro = new ADIS16470_IMU(); private MecanumDrive m_robotDrive; - - /** Creates a new TankDrivetrain. */ - public Drivetrain() { - // ADIS16470_IMU gyro = new ADIS16470_IMU(); private DifferentialDriveOdometry odometry; private DifferentialDriveKinematics kinematics; /** Creates a new TankDrivetrain. */ public Drivetrain() { + // ADIS16470_IMU gyro = new ADIS16470_IMU(); + piston = new DoubleSolenoid(1, PneumaticsModuleType.CTREPCM, pcm.PISTON_FORWARD, pcm.PISTON_REVERSE); @@ -70,32 +66,21 @@ public Drivetrain() { arriereGaucheEncoder = arrieregauche.getEncoder(); m_robotDrive.setSafetyEnabled(false); - // TODO: add where we start on the field? + avantDroitEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000); + arriereDroitEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000); + avantGaucheEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000); + arriereGaucheEncoder.setVelocityConversionFactor((12 / 66) * 4 * Math.PI * 2.54 / 6000); + + avantDroitEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100); + arriereDroitEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100); + avantGaucheEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100); + arriereGaucheEncoder.setPositionConversionFactor((12 / 66) * 4 * Math.PI * 2.45 / 100); + odometry = new DifferentialDriveOdometry( getRotation2d(), avantGaucheEncoder.getPosition(), avantDroitEncoder.getPosition()); kinematics = new DifferentialDriveKinematics(58.6); - - // à faire - AutoBuilder.configureRamsete( - this::getPose2d, - this::resetPose2d, - this::getChassisSpeeds, - this::drivePathplanner, - new ReplanningConfig(), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this); } public Pose2d getPose2d() { @@ -112,8 +97,12 @@ public ChassisSpeeds getChassisSpeeds() { } public DifferentialDriveWheelSpeeds getWheelSpeed() { - return new DifferentialDriveWheelSpeeds( - arriereGaucheEncoder.getVelocity() / 60, arriereDroitEncoder.getVelocity() / 60); + double leftEncoder = + (arriereGaucheEncoder.getVelocity() + avantGaucheEncoder.getVelocity()) / 2; + double rightEncoder = (arriereDroitEncoder.getVelocity() + avantDroitEncoder.getVelocity()) / 2; + + // TODO: check the division + return new DifferentialDriveWheelSpeeds(leftEncoder, rightEncoder); } public double getGyroAngle() { @@ -216,6 +205,13 @@ public void driveTank(double joystickDroit, double joystickGauche) { arrieredroit.set(joystickDroit); } + public void tankDriveVolts(double leftVolts, double rightVolts) { + avantdroit.setVoltage(rightVolts); + arrieredroit.setVoltage(rightVolts); + avantgauche.setVoltage(leftVolts); + arrieregauche.setVoltage(leftVolts); + } + @Override public void periodic() { // This method will be called once per scheduler diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index b811285..e5d6c7c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -24,7 +24,7 @@ public class Intake extends SubsystemBase { DigitalInput intakeLimitOut = new DigitalInput(degitalInput.INTAKE_LIMIT_SWITCH_OUT); DigitalInput intakeLimitIn = new DigitalInput(degitalInput.INTAKE_LIMIT_SWITCH_IN); - + public Intake() { intakeDroit = new CANSparkMax(deviceNumber.DeviceNumberIntakeDroit, MotorType.kBrushless); intakeGauche = new CANSparkMax(deviceNumber.DeviceNumberIntakeGauche, MotorType.kBrushless); @@ -57,7 +57,7 @@ public boolean getIntakeLimitOut() { public boolean getIntakeLimitIn() { return intakeLimitIn.get(); } - + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 2e1184c..c557637 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.util.ControlMode.*; - public class Limelight extends SubsystemBase { /** Creates a new Limelight. */ private NetworkTable networkTable;