Skip to content

Commit

Permalink
what ido
Browse files Browse the repository at this point in the history
  • Loading branch information
@Unclesdad
Unclesdad committed Jan 21, 2025
1 parent e49b656 commit 19ed0e4
Show file tree
Hide file tree
Showing 6 changed files with 76 additions and 244 deletions.
179 changes: 9 additions & 170 deletions simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,6 @@
"/Faults/Total Faults": "Alerts",
"/Robot/autos": "String Chooser",
"/Robot/drive/field2d": "Field2d",
"/Robot/drive/frontLeft/driveFeedback": "PIDController",
"/Robot/drive/frontLeft/turnFeedback": "PIDController",
"/Robot/drive/frontRight/driveFeedback": "PIDController",
"/Robot/drive/frontRight/turnFeedback": "PIDController",
"/Robot/drive/rearLeft/driveFeedback": "PIDController",
"/Robot/drive/rearLeft/turnFeedback": "PIDController",
"/Robot/drive/rearRight/driveFeedback": "PIDController",
"/Robot/drive/rearRight/turnFeedback": "PIDController",
"/Robot/drive/rotationController": "PIDController",
"/Robot/drive/translationController": "ProfiledPIDController",
"/Robot/elevator/measurement/mech": "Mechanism2d",
Expand All @@ -29,156 +21,39 @@
"/SmartDashboard/Alerts": "Alerts",
"/SmartDashboard/Scheduler": "Scheduler",
"/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d",
"/SmartDashboard/drive dynamic backward": "Command",
"/SmartDashboard/drive dynamic forward": "Command",
"/SmartDashboard/drive quasistatic backward": "Command",
"/SmartDashboard/drive quasistatic forward": "Command",
"/SmartDashboard/rotation dynamic backward": "Command",
"/SmartDashboard/rotation dynamic forward": "Command",
"/SmartDashboard/rotation quasistatic backward": "Command",
"/SmartDashboard/rotation quasistatic forward": "Command",
"/SmartDashboard/translation dynamic backward": "Command",
"/SmartDashboard/translation dynamic forward": "Command",
"/SmartDashboard/translation quasistatic backward": "Command",
"/SmartDashboard/translation quasistatic forward": "Command",
"/SmartDashboard/turn dynamic backward": "Command",
"/SmartDashboard/turn dynamic forward": "Command",
"/SmartDashboard/turn quasistatic backward": "Command",
"/SmartDashboard/turn quasistatic forward": "Command"
"/SmartDashboard/translation quasistatic forward": "Command"
},
"windows": {
"/Robot/autos": {
"window": {
"visible": true
}
},
"/Robot/drive/field2d": {
"bottom": 1476,
"height": 8.210550308227539,
"left": 150,
"module- FL": {
"style": "Line"
},
"module- RR": {
"style": "Line"
},
"module-FR": {
"style": "Line"
},
"module-RL": {
"style": "Line"
},
"right": 2961,
"top": 79,
"width": 16.541748046875,
"window": {
"visible": true
}
},
"/Robot/elevator/measurement/mech": {
"window": {
"visible": true
}
},
"/Robot/elevator/setpoint/mech": {
"bottom": 1638,
"builtin": "2025 Reefscape",
"height": 8.051901817321777,
"left": 534,
"right": 3466,
"top": 291,
"width": 17.54825210571289,
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"Retained Values": {
"open": false
},
"retained": {
"Robot": {
"open": true
}
},
"transitory": {
"Robot": {
"drive": {
"ChassisSpeeds##v_/Robot/drive/robotRelativeChassisSpeeds": {
"open": true
},
"Pose2d##v_/Robot/drive/getPose": {
"Rotation2d##v_rotation": {
"open": true
},
"open": true
},
"Pose2d##v_/Robot/drive/pose": {
"Rotation2d##v_rotation": {
"open": true
},
"Translation2d##v_translation": {
"open": true
},
"open": true
},
"SwerveModulePosition[]##v_/Robot/drive/getModulePositions": {
"SwerveModulePosition##v_[1]": {
"Rotation2d##v_angle": {
"open": true
}
}
},
"SwerveModuleState[]##v_/Robot/drive/getModuleSetpoints": {
"SwerveModuleState##v_[0]": {
"Rotation2d##v_angle": {
"open": true
}
},
"SwerveModuleState##v_[1]": {
"open": true
},
"SwerveModuleState##v_[2]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"SwerveModuleState##v_[3]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"SwerveModuleState[]##v_/Robot/drive/getModuleStates": {
"SwerveModuleState##v_[0]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"frontRight": {
"SwerveModulePosition##v_/Robot/drive/frontRight/position": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"SwerveModuleState##v_/Robot/drive/frontRight/state": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"open": true
},
"open": true,
"vision": {
"open": true
}
},
"Shuffleboard": {
"open": true
}
}
Expand All @@ -196,43 +71,7 @@
0.0,
0.8500000238418579
],
"height": 0,
"series": [
{
"color": [
0.2980392277240753,
0.44705885648727417,
0.6901960968971252,
1.0
],
"id": "NT:/Robot/drive/pose/translation/x"
},
{
"color": [
0.8666667342185974,
0.5176470875740051,
0.32156863808631897,
1.0
],
"id": "NT:/Robot/drive/pose/translation/y"
}
]
}
],
"window": {
"name": "Elevator"
}
},
"Plot <1>": {
"plots": [
{
"backgroundColor": [
0.0,
0.0,
0.0,
0.8500000238418579
],
"height": 338,
"height": 285,
"series": [
{
"color": [
Expand Down
19 changes: 16 additions & 3 deletions src/main/java/org/sciborgs1155/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,9 @@
import org.sciborgs1155.robot.scoral.Scoral;
import org.sciborgs1155.robot.vision.Vision;

import com.pathplanner.lib.pathfinding.LocalADStar;
import com.pathplanner.lib.pathfinding.Pathfinding;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
Expand Down Expand Up @@ -90,6 +93,8 @@ private void configureGameBehavior() {

RobotController.setBrownoutVoltage(6.0);

Pathfinding.setPathfinder(new LocalADStar());

if (isReal()) {
URCL.start();
pdh.clearStickyFaults();
Expand Down Expand Up @@ -153,9 +158,17 @@ private void configureBindings() {
.onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));

// TODO: Add any additional bindings.
driver.a().onTrue(Commands.defer( () -> align.pathfollow(align.pathfind(new Pose2d(2, 2, new Rotation2d()))), Set.of(drive)));

//driver.b().onTrue(align.pathfollow(align.pathfind(new Pose2d(100, 4, new Rotation2d()))));
// driver
// .a()
// .onTrue(
// Commands.defer(
// () -> align.directPathfollow(align.directPathfind(new Pose2d(2, 2, new Rotation2d()))),
// Set.of(drive)));

driver.a().onTrue(Commands.defer(() -> align.pathfind(align.directPathfind(new Pose2d(2,2,Rotation2d.fromDegrees(20)))), Set.of(drive)));
driver.b().onTrue(Commands.defer(() -> align.directPathfollow(align.directPathfind(new Pose2d(2,2,Rotation2d.fromDegrees(20)))), Set.of(drive)));

// driver.b().onTrue(align.pathfollow(align.pathfind(new Pose2d(100, 4, new Rotation2d()))));
}

/**
Expand Down
58 changes: 25 additions & 33 deletions src/main/java/org/sciborgs1155/robot/commands/Alignment.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,24 @@
import static org.sciborgs1155.robot.Constants.Field.*;
import static org.sciborgs1155.robot.drive.DriveConstants.*;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.FollowPathCommand;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.path.GoalEndState;
import com.pathplanner.lib.path.IdealStartingState;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.path.Waypoint;
import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState;
import com.pathplanner.lib.util.DriveFeedforwards;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;

import java.util.List;
import java.util.Set;

import org.sciborgs1155.robot.Constants.Field.Branch;
import org.sciborgs1155.robot.Constants.Field.Level;
import org.sciborgs1155.robot.drive.Drive;
Expand Down Expand Up @@ -52,7 +56,7 @@ public Alignment(Drive drive, Elevator elevator, Scoral scoral) {
* @return A command to quickly prepare and then score in the reef.
*/
public Command reef(Level level, Branch branch) {
return pathfollow(pathfind(branch.pose))
return directPathfollow(directPathfind(branch.pose))
.alongWith(elevator.scoreLevel(level).until(() -> elevator.atPosition(level.height)))
.andThen(scoral.outtake());
}
Expand All @@ -66,7 +70,7 @@ public Command reef(Level level, Branch branch) {
* @return A command to score in the reef without raising the elevator while moving.
*/
public Command safeReef(Level level, Branch branch) {
return pathfollow(pathfind(branch.pose))
return directPathfollow(directPathfind(branch.pose))
.andThen(elevator.scoreLevel(level).until(() -> elevator.atPosition(level.height)))
.andThen(scoral.outtake());
}
Expand All @@ -88,63 +92,51 @@ public Command nearReef(Level level) {
* @return A command to align with the human player station source.
*/
public Command source() {
return pathfollow(pathfind(drive.pose().nearest(List.of(LEFT_SOURCE, RIGHT_SOURCE))))
return directPathfollow(directPathfind(drive.pose().nearest(List.of(LEFT_SOURCE, RIGHT_SOURCE))))
.alongWith(elevator.retract());
}

public Command processor() {
return pathfollow(pathfind(PROCESSOR));
return directPathfollow(directPathfind(PROCESSOR));
// TODO idk how this would work. research this a bit more
}

public Command cage() {
return pathfollow(pathfind(nearestCage(drive.pose())));
return directPathfollow(directPathfind(nearestCage(drive.pose())));
// TODO it should do more. when we have a plan, update this
}

/**
* Creates a pathplanner path to a goal (while avoiding obstacles).
* Creates a pathplanner path to a goal.
*
* @param goal The goal ending pose for the robot.
* @return A pathplanner path to get to a field position.
*/
public PathPlannerPath pathfind(Pose2d goal) {
PathConstraints constraints =
new PathConstraints(MAX_SPEED, MAX_ACCEL, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL);
var speeds = drive.robotRelativeChassisSpeeds();
double currentSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
public PathPlannerPath directPathfind(Pose2d goal) {
List<Waypoint> waypoints = PathPlannerPath.waypointsFromPoses(drive.pose(), goal);
double speed = Math.hypot(drive.robotRelativeChassisSpeeds().vxMetersPerSecond, drive.robotRelativeChassisSpeeds().vyMetersPerSecond);
PathPlannerPath path =
new PathPlannerPath(
waypoints,
constraints,
new IdealStartingState(currentSpeed, drive.heading()),
new GoalEndState(0, goal.getRotation()));
waypoints, PATH_CONSTRAINTS, new IdealStartingState(speed, drive.heading()), new GoalEndState(0, goal.getRotation()));
path.preventFlipping = true;
List<PathPlannerTrajectoryState> states = path.generateTrajectory(drive.robotRelativeChassisSpeeds(), drive.heading(), ROBOT_CONFIG).getStates();

for (int i = 0; i < states.size(); i++) {
System.out.println(states.get(i).linearVelocity);
}
return path;
}

public Command pathfind(PathPlannerPath path) {
// PathPlannerPath path = directPathfind(goal);
// System.out.println(path.getPoint(1));
// System.out.println(path.generateTrajectory(drive.robotRelativeChassisSpeeds(), drive.heading(), ROBOT_CONFIG).getStates().size());
// System.out.println(path.getAllPathPoints().size() + "\n");
return AutoBuilder.pathfindThenFollowPath(path, PATH_CONSTRAINTS);
}

/**
* Follows a given pathplanner path.
*
* @param path A pathplanner path.
* @return A command to follow a path.
*/
public Command pathfollow(PathPlannerPath path) {
System.out.println(
"Waypoint 1: "
+ path.getWaypoints().get(path.getWaypoints().size() - 2).anchor().toString());
System.out.println(
"Waypoint 2: "
+ path.getWaypoints().get(path.getWaypoints().size() - 1).anchor().toString());
System.out.println(
path.generateTrajectory(drive.robotRelativeChassisSpeeds(), drive.heading(), ROBOT_CONFIG)
.getTotalTimeSeconds());
public Command directPathfollow(PathPlannerPath path) {

return new FollowPathCommand(
path,
Expand All @@ -157,6 +149,6 @@ public Command pathfollow(PathPlannerPath path) {
new PIDConstants(Rotation.P, Rotation.I, Rotation.D)),
ROBOT_CONFIG,
() -> false,
drive);
drive).withInterruptBehavior(InterruptionBehavior.kCancelSelf);
}
}
Loading

0 comments on commit 19ed0e4

Please sign in to comment.