diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b8186d75..223f70d1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -3,19 +3,32 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; public interface Drive { + /** + * Sets the desired chassis speed + * + * @param speeds specifies the setpoint speed for the chassis + */ public void runVelocity(ChassisSpeeds speeds); + /** + * This returns the robot's max linear speed in meter/sec + * + * @return speed in meter/sec + */ public double getMaxLinearSpeed(); + /** + * This returns the robot's max angular speed in radian/sec + * + * @return speed in radian/sec + */ public double getMaxAngularSpeed(); /* public void stop(); - public Pose2d getPose(); - public void setPose(Pose2d Pose2d); - public Rotation2d getRotation(); */ + } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index 120e9f55..9c735466 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,19 +1,34 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; public class DriveBase extends SubsystemBase implements Drive { + protected static final DifferentialDriveKinematics kinematics = + new DifferentialDriveKinematics(Units.inchesToMeters(27.0)); + + @AutoLogOutput protected double leftVelocityMetersPerSecond; + @AutoLogOutput protected double rightVelocityMetersPerSecond; + public DriveBase() {} - public void runVelocity(ChassisSpeeds speeds) {} - ; + public void runVelocity(ChassisSpeeds chassisSpeeds) { + // Convert to wheel speeds + DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + + leftVelocityMetersPerSecond = wheelSpeeds.leftMetersPerSecond; + rightVelocityMetersPerSecond = wheelSpeeds.rightMetersPerSecond; + } public double getMaxLinearSpeed() { - return 0; + return 5; } public double getMaxAngularSpeed() { - return 0; + return 5; } }