Skip to content

Commit

Permalink
swerve sim works, vision doesnt
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ani-8712
Ani-8712 committed Dec 22, 2024
1 parent ca1213e commit f3f9330
Show file tree
Hide file tree
Showing 20 changed files with 465 additions and 86 deletions.
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,5 +25,6 @@
}
},
],
"java.test.defaultConfig": "WPIlibUnitTests"
"java.test.defaultConfig": "WPIlibUnitTests",
"java.debug.settings.onBuildFailureProceed": true
}
13 changes: 4 additions & 9 deletions build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ dependencies {
implementation "gov.nist.math:jama:1.0.3"

def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
}

test {
Expand Down Expand Up @@ -119,12 +119,7 @@ repositories {
mavenLocal()
}

configurations.all {
exclude group: "edu.wpi.first.wpilibj"
}

task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
mainClass = "org.littletonrobotics.junction.CheckInstall"
task(replayWatch, type: JavaExec) {
mainClass = "org.littletonrobotics.junction.ReplayWatch"
classpath = sourceSets.main.runtimeClasspath
}
compileJava.finalizedBy checkAkitInstall
}
98 changes: 98 additions & 0 deletions simgui-ds.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
{
"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
}
],
"robotJoysticks": [
{
"guid": "78696e70757401000000000000000000",
"useGamepad": true
}
]
}
109 changes: 101 additions & 8 deletions src/main/java/team3647/frc2024/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.Set;
import java.util.function.BiFunction;
import java.util.function.BooleanSupplier;
Expand Down Expand Up @@ -173,14 +174,22 @@ public class AutoCommands implements AllianceObserver {

public final AutonomousMode blueFour_S2N1N2N3;

public final AutonomousMode s15tof1;

public final Trajectory<SwerveSample> emptyTraj = new Trajectory<SwerveSample>("Empty", new ArrayList<>(), new ArrayList<>(), new ArrayList<>());

public List<AutonomousMode> redAutoModes;

public List<AutonomousMode> blueAutoModes;



private MidlineNote lastNote = MidlineNote.ONE;

Alliance color = Alliance.Red;

boolean isRed = color == Alliance.Red;

private final PIDController fastXController = new PIDController(2, 0, 0);

// public final AutonomousMode yes;
Expand Down Expand Up @@ -328,6 +337,8 @@ public AutoCommands(
this.blueFour_S2N1N2N3 =
new AutonomousMode(
four_S2N1N2N3(Alliance.Blue), getInitial(s2_to_n1), "blue close 4 s2");
this.s15tof1 =
new AutonomousMode(s15ToF1(), getInitialAuto(s15_to_f1));

redAutoModes =
new ArrayList<AutonomousMode>(
Expand All @@ -341,7 +352,8 @@ public AutoCommands(
redFour_S3F5F4F3,
redSix_S1F1F2N1N2N3,
redTwo_S2F3,
doNothing));
doNothing,
s15tof1));

blueAutoModes =
new ArrayList<AutonomousMode>(
Expand Down Expand Up @@ -370,6 +382,7 @@ public enum MidlineNote {
@Override
public void onAllianceFound(Alliance color) {
this.color = color;
isRed = this.color == Alliance.Red;
}

public void registerCommands() {}
Expand Down Expand Up @@ -777,10 +790,17 @@ public Command scorePreload() {
}

public Pose2d getInitial(String path) {
Trajectory traj = Choreo.loadTrajectory(path).get();
var maybetraj = Choreo.loadTrajectory(path);
if(maybetraj.isEmpty()) return new Pose2d();
var traj = maybetraj.get();
return traj.getInitialPose(false);
}

public Pose2d getInitialAuto(String path){
Trajectory traj = getTraj(path);
return traj.getInitialPose(isRed);
}

public Pose2d getFinal(String path) {
Trajectory traj = Choreo.loadTrajectory(path).get();
return traj.getFinalPose(false);
Expand All @@ -790,6 +810,14 @@ public Command spinUp() {
return Commands.parallel(superstructure.spinUp(), superstructure.feed());
}

public Trajectory<SwerveSample> getTraj(String path){
Optional<Trajectory<SwerveSample>> maybeTraj = Choreo.loadTrajectory(path);
if(maybeTraj.isPresent()){
return maybeTraj.get();
}
return emptyTraj;
}

// public Command target() {
// return Commands.run(() -> swerve.drive(0, 0, deeThetaOnTheMove()),
// swerve).withTimeout(2);
Expand Down Expand Up @@ -831,8 +859,64 @@ public Command target() {
}

public Command followChoreoPathWithOverrideFast(String path, Alliance color) {
Trajectory traj = Choreo.loadTrajectory(path).get();
Trajectory traj = getTraj(path);
boolean mirror = color == Alliance.Red;
try {

PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
} catch (Exception e) {

}
// Logger.recordOutput("Autos/current path", path);
return customChoreoFolloweForOverride(
traj,
swerve::getOdoPose,
choreoSwerveController(
AutoConstants.kXController,
AutoConstants.kYController,
new PIDController(0, 0, 0)),
(ChassisSpeeds speeds) ->
swerve.drive(
(!hasPiece
&& hasTarget.getAsBoolean()
&& swerve.getOdoPose().getX()
> (color == Alliance.Blue
? 5
: FieldConstants
.kFieldLength
- 8.75)
&& swerve.getOdoPose().getX()
< (color == Alliance.Blue
? 8.75
: FieldConstants
.kFieldLength
- 5))
? autoDriveVelocities.get().dx * SLOWDOWN
: speeds.vxMetersPerSecond * SLOWDOWN,
(!hasPiece
&& hasTarget.getAsBoolean()
&& swerve.getOdoPose().getX()
> (color == Alliance.Blue
? 5
: FieldConstants
.kFieldLength
- 8.75)
&& swerve.getOdoPose().getX()
< (color == Alliance.Blue
? 8.75
: FieldConstants
.kFieldLength
- 5))
? autoDriveVelocities.get().dy * SLOWDOWN
: speeds.vyMetersPerSecond * SLOWDOWN,
deeTheta()),
() -> mirror)
.andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
}

public Command followChoreoPathWithOverrideFast(String path) {
Trajectory traj = getTraj(path);
boolean mirror = isRed;
try {
PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
} catch (Exception e) {
Expand Down Expand Up @@ -885,9 +969,13 @@ public Command followChoreoPathWithOverrideFast(String path, Alliance color) {
.andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
}

public Command s15ToF1(){
return followChoreoPathWithOverrideFast(f1_to_f2);
}

public Command followChoreoPathWithOverrideFastOnTheMove(String path, Alliance color) {
Trajectory traj = Choreo.loadTrajectory(path).get();
boolean mirror = color == Alliance.Red;
boolean mirror = isRed;
try {
PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
} catch (Exception e) {
Expand Down Expand Up @@ -940,8 +1028,13 @@ public Command followChoreoPathWithOverrideFastOnTheMove(String path, Alliance c
}

public Command followChoreoPathWithOverride(String path, Alliance color) {
Trajectory traj = Choreo.loadTrajectory(path).get();
boolean mirror = color == Alliance.Red;
var maybeTraj = Choreo.loadTrajectory(path);

if(maybeTraj.isEmpty()) return Commands.none();

var traj = maybeTraj.get();

boolean mirror = isRed;
try {
PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
} catch (Exception e) {
Expand Down Expand Up @@ -969,7 +1062,7 @@ public Command followChoreoPathWithOverride(String path, Alliance color) {
}

public Command followChoreoPathWithOverrideNoverrideFast(String path, Alliance color) {
Trajectory traj = Choreo.loadTrajectory(path).get();
Trajectory traj = getTraj(path);
boolean mirror = color == Alliance.Red;
try {
PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
Expand Down Expand Up @@ -1014,7 +1107,7 @@ public Command followChoreoPathWithOverrideNoverrideFast(String path, Alliance c
}

public Command followChoreoPathWithOverrideLongTimer(String path, Alliance color) {
Trajectory traj = Choreo.loadTrajectory(path).get();
Trajectory traj = getTraj(path);
boolean mirror = color == Alliance.Red;
try {
PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/auto/AutonomousMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public AutonomousMode(Command autoCommand, Pose2d ppinitial) {
Elastic.sendAlert(
new ElasticNotification(
NotificationLevel.WARNING, "Unamed Auto!",
"One of your autos is Unamed! Object ID: " + this.toString()));
"One of your autos is Unamed! Object ID: " + autoCommand.getName()));
}

private final Command autoCommand;
Expand Down
12 changes: 7 additions & 5 deletions src/main/java/team3647/frc2024/commands/DrivetrainCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
Expand Down Expand Up @@ -31,18 +32,19 @@ public Command driveVisionTeleop(
return Commands.run(
() -> {
int invert = 1;
if (DriverStation.getAlliance().isPresent()) {
if (DriverStation.getAlliance().get() == Alliance.Red) {
invert = -1;
}
}
boolean enabeld = autoDriveEnabled.getAsBoolean();
DriveMode mode = autoDriveMode.get();
Twist2d autoDriveTwist2d = autoDriveVelocities.get();
double triggerSlow = slowTriggerFunction.getAsBoolean() ? 0.6 : 1;
// boolean autoSteer = enableAutoSteer.getAsBoolean();
boolean fieldOriented = getIsFieldOriented.getAsBoolean();
boolean openloop = true;

if (DriverStation.getAlliance().isPresent() && RobotBase.isSimulation()) {
if (DriverStation.getAlliance().get() == Alliance.Red) {
invert = -1;
}
}
double y =
Math.pow(ySpeedFunction.getAsDouble(), 2)
* Math.signum(ySpeedFunction.getAsDouble())
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/team3647/frc2024/constants/PivotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,10 +63,10 @@ public class PivotConstants {
public static final Transform2d robotToPivot2d =
new Transform2d(new Translation2d(Units.inchesToMeters(0.3), 0), new Rotation2d());

public static final TimeOfFlight tofBack =
new TimeOfFlight(GlobalConstants.SensorIds.pivotBackId);
public static final TimeOfFlight tofFront =
new TimeOfFlight(GlobalConstants.SensorIds.pivotFrontId);
// public static final TimeOfFlight tofBack =
// new TimeOfFlight(GlobalConstants.SensorIds.pivotBackId);
// public static final TimeOfFlight tofFront =
// new TimeOfFlight(GlobalConstants.SensorIds.pivotFrontId);

// distance vs pivot angle
public static final InterpolatingDoubleTreeMap kMasterSpeakerMap =
Expand Down
Loading

0 comments on commit f3f9330

Please sign in to comment.