diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 86b1d84..b23b040 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -75,6 +75,8 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); + m_robotContainer.resetOdometry(); + /* * String autoSelected = SmartDashboard.getString("Auto Selector", * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d77076e..408712f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,13 +7,26 @@ package frc4388.robot; +import java.util.List; +import java.util.function.BiConsumer; + import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; @@ -146,14 +159,49 @@ public class RobotContainer { m_robotDrive.setDriveTrainNeutralMode(mode); } + public void resetOdometry() { + m_robotDrive.resetGyroAngles(); + m_robotDrive.setOdometry(new Pose2d()); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto - return new InstantCommand(); + + var wheelSpeeds = new ChassisSpeeds(); + + // Create config for trajectory + TrajectoryConfig config = new TrajectoryConfig( DriveConstants.kMaxSpeedMetersPerSecond, + DriveConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(DriveConstants.kDriveKinematics); + + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of( + new Translation2d(1, 1), + new Translation2d(2, -1) + ), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + // Pass config + config); + + RamseteCommand ramseteCommand = new RamseteCommand( + exampleTrajectory, + m_robotDrive::getPose, + new RamseteController(), + DriveConstants.kDriveKinematics, + m_robotDrive::tankDriveVelocity, + m_robotDrive); + + // Run path following command, then stop at the end. + return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index f8fdd37..73a358d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -25,12 +25,14 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Gains; @@ -61,7 +63,7 @@ public class Drive extends SubsystemBase { public long m_lastTime, m_deltaTime; //in milliseconds - public double m_lastAngleYaw, m_currentAngleYaw; + public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle; /** * Add your docs here. @@ -364,6 +366,29 @@ public class Drive extends SubsystemBase { runDriveStraightVelocityPID(0, targetGyro); } + /** + * Controls the left and right sides of the drive with velocity targets. + * + * @param leftSpeed the commanded left output + * @param rightSpeed the commanded right output + */ + public void tankDriveVelocity(double leftSpeed, double rightSpeed) { + DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed); + ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds); + double moveVelMPS = chassisSpeeds.vxMetersPerSecond; + double angleVelRad = chassisSpeeds.omegaRadiansPerSecond; + double angleVelDeg = Math.toDegrees(angleVelRad); + + m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000); + m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle, + m_currentAngleYaw-(360), + m_currentAngleYaw+(360)); + double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; + double moveVel = inchesToMeters(metersToInches(moveVelMPS))/DriveConstants.SECONDS_TO_TICK_TIME; + + runDriveStraightVelocityPID(moveVel, targetGyro); + } + /** * Returns the current yaw of the pigeon */ @@ -400,8 +425,16 @@ public class Drive extends SubsystemBase { public void resetGyroYaw() { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); + resetGyroAngles(); + } + + /** + * Add docs here + */ + public void resetGyroAngles() { m_lastAngleYaw = 0; m_currentAngleYaw = 0; + m_kinematicsTargetAngle = 0; } /** @@ -484,6 +517,15 @@ public class Drive extends SubsystemBase { return ticks * DriveConstants.INCHES_PER_TICK; } + /** + * Converts a value in inches to ticks. + * @param inches The value in inches to convert + * @return The converted value in ticks + */ + public double inchesToTicks(double inches) { + return inches * DriveConstants.TICKS_PER_INCH; + } + /** * Converts a value in inches to meters. * @param inches The value in inches to convert @@ -492,6 +534,15 @@ public class Drive extends SubsystemBase { public double inchesToMeters(double inches) { return inches / DriveConstants.INCHES_PER_METER; } + + /** + * Converts a value in meters to inches. + * @param meters The value in meters to convert + * @return The converted value in inches + */ + public double metersToInches(double meters) { + return meters * DriveConstants.INCHES_PER_METER; + } /* * Set to high or low gear based on boolean state, true = high, false = low