Skip to content

Commit

Permalink
Merge branch 'maiden-phr' of https://github.com/Team3128/3128-robot-2024
Browse files Browse the repository at this point in the history 
 into maiden-phr
  • Loading branch information
@bloobglob
bloobglob committed Mar 6, 2024
2 parents f1458fc + cfb76c4 commit a0b95c8
Show file tree
Hide file tree
Showing 4 changed files with 71 additions and 28 deletions.
10 changes: 6 additions & 4 deletions src/main/java/frc/team3128/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ public static class AutoConstants {
}

public static class SwerveConstants {
public static final double RAMP_TIME = 1;

public static final int pigeonID = 30;

/* Drivetrain Constants */
Expand Down Expand Up @@ -106,7 +108,7 @@ public static class SwerveConstants {
// Theoretical: v = 4.96824, omega = 11.5
// Real: v = 4.5, omega = 10
// For safety, use less than theoretical and real values
public static final double maxSpeed = 5.5;//4.8; //meters per second - 16.3 ft/sec
public static final double maxSpeed = 5.9;//4.8; //meters per second - 16.3 ft/sec
public static final double maxAttainableSpeed = maxSpeed; //Stole from citrus.
public static final double maxAcceleration = 3;
public static final double maxAngularVelocity = 8; //3; //11.5; // citrus: 10 - Mason look at this later wtf
Expand Down Expand Up @@ -305,6 +307,8 @@ public static class ShooterConstants {

public static final double SHOOTER_TEST_PLATEAU = 1;
public static final double SHOOTER_TEST_TIMEOUT = 1.8;

public static final double PROJECTILE_SPEED = 100; // m/s
}

public static class ClimberConstants {
Expand All @@ -314,7 +318,7 @@ public static class ClimberConstants {
public static final Constraints TRAP_CONSTRAINTS = new Constraints(MAX_VELOCTIY, MAX_ACCELERATION);
public static final int LEFT_MOTOR_ID = 21;
public static final int RIGHT_MOTOR_ID = 22;
public static final double GEAR_RATIO = 1.0 / 12.0;
public static final double GEAR_RATIO = 1.0 / 20.0;
public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(1.751) * Math.PI;
public static final double POSITION_TOLERANCE = 0.5;
public static final double PIVOT_CLIMBER_DIST = 0.28;
Expand Down Expand Up @@ -422,8 +426,6 @@ public enum Colors {

}
}


}


12 changes: 6 additions & 6 deletions src/main/java/frc/team3128/commands/CmdManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,17 +37,17 @@ public static Command vibrateController(){

public static Command autoShoot() {
return sequence(
runOnce(()-> DriverStation.reportWarning("AutoShoot: CommandStarting", false)),
// runOnce(()-> DriverStation.reportWarning("AutoShoot: CommandStarting", false)),
parallel(
rampUp(),
swerve.turnInPlace().asProxy().withTimeout(1)
swerve.turnInPlace(true).asProxy().withTimeout(1)
// runOnce(()-> CmdSwerveDrive.setTurnSetpoint(swerve.getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue))),
// waitUntil(()-> CmdSwerveDrive.rController.atSetpoint())
),
intake.intakeRollers.outtakeNoRequirements(),
waitSeconds(0.1),
neutral(false),
runOnce(()-> DriverStation.reportWarning("AutoShoot: CommandEnding", false))
neutral(false)
// runOnce(()-> DriverStation.reportWarning("AutoShoot: CommandEnding", false))
);
}

Expand Down Expand Up @@ -95,7 +95,7 @@ public static Command feed(double rpm, double height){
}

public static Command rampUp() {
return rampUp(ShooterConstants.MAX_RPM, ()-> climber.interpolate(swerve.getDist()));
return rampUp(ShooterConstants.MAX_RPM, ()-> climber.interpolate(swerve.getPredictedDistance()));
}

public static Command rampUp(double rpm, double height){
Expand All @@ -118,7 +118,7 @@ public static Command rampUpAmp() {
}

public static Command rampUpContinuous() {
return rampUpContinuous(ShooterConstants.MAX_RPM, ()-> climber.interpolate(swerve.getDist()));
return rampUpContinuous(ShooterConstants.MAX_RPM, ()-> climber.interpolate(swerve.getPredictedDistance()));
}

public static Command rampUpContinuous(double rpm, DoubleSupplier height) {
Expand Down
24 changes: 12 additions & 12 deletions src/main/java/frc/team3128/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,13 +108,13 @@ public Command miniOuttake() {

public Command serialize() {
return sequence(
runOnce(()-> DriverStation.reportWarning("Serialize: CommandStarting", false)),
// runOnce(()-> DriverStation.reportWarning("Serialize: CommandStarting", false)),
runManipulator(-0.1),
waitUntil(()-> !hasObjectPresent()),
runManipulator(0.1),
waitUntil(()-> hasObjectPresent()),
runManipulator(0),
runOnce(()-> DriverStation.reportWarning("Serialize: CommandEnding", false))
runManipulator(0)
// runOnce(()-> DriverStation.reportWarning("Serialize: CommandEnding", false))
);
}

Expand Down Expand Up @@ -160,7 +160,7 @@ private Intake(){

public Command retract(boolean shouldStall) {
return sequence(
runOnce(()-> DriverStation.reportWarning("Retract: CommandStarting", false)),
// runOnce(()-> DriverStation.reportWarning("Retract: CommandStarting", false)),
runOnce(()-> Leds.getInstance().setLedColor(Colors.PIECE)),
// CmdManager.vibrateController(),
runOnce(()-> isRetracting = true),
Expand All @@ -179,32 +179,32 @@ public Command retract(boolean shouldStall) {
intakePivot.reset(0),
runOnce(()-> Leds.getInstance().setDefaultColor())
)
),
runOnce(()-> DriverStation.reportWarning("Retract: CommandEnding", false))
)
// runOnce(()-> DriverStation.reportWarning("Retract: CommandEnding", false))
);
}

public Command intake(Setpoint setpoint) {
return sequence(
runOnce(()-> DriverStation.reportWarning("Intake: CommandStarting", false)),
// runOnce(()-> DriverStation.reportWarning("Intake: CommandStarting", false)),
runOnce(()-> isRetracting = true),
intakeRollers.runManipulator(INTAKE_POWER),
intakePivot.pivotTo(setpoint.angle),
intakeRollers.intake(),
retract(true),
runOnce(()-> DriverStation.reportWarning("Intake: CommandEnding", false))
retract(true)
// runOnce(()-> DriverStation.reportWarning("Intake: CommandEnding", false))
);
}

public Command outtake() {
return sequence (
runOnce(()-> DriverStation.reportWarning("Outtake: CommandStarting", false)),
// runOnce(()-> DriverStation.reportWarning("Outtake: CommandStarting", false)),
intakePivot.pivotTo(Setpoint.EXTENDED.angle),
waitUntil(()-> intakePivot.atSetpoint()),
intakeRollers.outtake(),
waitSeconds(0.5),
retract(false),
runOnce(()-> DriverStation.reportWarning("Outtake: CommanedEnding", false))
retract(false)
// runOnce(()-> DriverStation.reportWarning("Outtake: CommanedEnding", false))
);
}

Expand Down
53 changes: 47 additions & 6 deletions src/main/java/frc/team3128/subsystems/Swerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,13 @@
import common.utility.shuffleboard.NAR_Shuffleboard;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.team3128.Robot;
import frc.team3128.RobotContainer;
import frc.team3128.Constants.ShooterConstants;
import frc.team3128.commands.CmdSwerveDrive;

import static frc.team3128.Constants.SwerveConstants.*;
Expand Down Expand Up @@ -87,25 +89,64 @@ public void zeroGyro(double reset) {
gyro.setYaw(reset);
}

public double getPredictedDistance() {
final ChassisSpeeds velocity = getFieldVelocity();
final Translation2d predictedPos = getPredictedPosition(velocity, RAMP_TIME);
final double shotTime = getProjectileTime(getDist(predictedPos));
final Translation2d target = calculateTarget(Robot.getAlliance() == Alliance.Red ? speakerMidpointRed : speakerMidpointBlue, velocity, shotTime);
final double distance = getDist(predictedPos, target);
return distance;
}

public double getPredictedAngle() {
final ChassisSpeeds velocity = getFieldVelocity();
final Translation2d predictedPos = getPredictedPosition(velocity, RAMP_TIME);
final double shotTime = getProjectileTime(getDist(predictedPos));
final Translation2d target = calculateTarget(Robot.getAlliance() == Alliance.Red ? speakerMidpointRed : speakerMidpointBlue, velocity, shotTime);
final double angle = getTurnAngle(predictedPos, target);
return angle;
}

public Translation2d getPredictedPosition(ChassisSpeeds velocity, double time) {
final Translation2d currentPosition = getPose().getTranslation();
return currentPosition.plus(new Translation2d(velocity.vxMetersPerSecond * time, velocity.vyMetersPerSecond * time));
}

public double getProjectileTime(double distance) {
return distance / ShooterConstants.PROJECTILE_SPEED;
}

public Translation2d calculateTarget(Translation2d target, ChassisSpeeds velocity, double time) {
return target.minus(new Translation2d(0, velocity.vyMetersPerSecond * time));
}

public double getDist() {
return getDist(Robot.getAlliance() == Alliance.Red ? speakerMidpointRed : speakerMidpointBlue);
}

public double getDist(Translation2d point) {
return getPose().getTranslation().getDistance(point) - robotLength / 2.0;
return getDist(getPose().getTranslation(), point);
}

public double getDist(Translation2d point1, Translation2d point2) {
return point1.getDistance(point2) - robotLength / 2.0;
}

public double getTurnAngle() {
return getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue);
}

public double getTurnAngle(Translation2d point) {
Translation2d pos = Swerve.getInstance().getPose().getTranslation();
return Math.toDegrees(Math.atan2(point.getY() - pos.getY(), point.getX() - pos.getX())) + angleOffset;
public double getTurnAngle(Translation2d target) {
final Translation2d robotPos = Swerve.getInstance().getPose().getTranslation();
return getTurnAngle(robotPos, target);
}

public double getTurnAngle(Translation2d robotPos, Translation2d targetPos) {
return Math.toDegrees(Math.atan2(targetPos.getY() - robotPos.getY(), targetPos.getX() - robotPos.getX())) + angleOffset;
}

public Command turnInPlace() {
return turnInPlace(()-> getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue));
public Command turnInPlace(boolean moving) {
return turnInPlace(()-> moving ? getPredictedAngle() : getTurnAngle());
}

public Command turnInPlace(DoubleSupplier setpoint) {
Expand Down

0 comments on commit a0b95c8

Please sign in to comment.