Skip to content

Commit

Permalink
Merge pull request #3 from FRCTeam3044/poses
Browse files Browse the repository at this point in the history 
Poses/Button Board
  • Loading branch information
@nab138
nab138 authored Feb 12, 2025
2 parents 232a946 + bc9ce2a commit 64decff
Show file tree
Hide file tree
Showing 16 changed files with 494 additions and 376 deletions.
146 changes: 133 additions & 13 deletions src/main/deploy/config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,11 @@
"comment": "Auto-Generated",
"value": "0.0"
},
"Coral proximity distance": {
"Pathfinding Y Controller/I": "0.0",
"Algae low scoring dist": {
"comment": "Auto-Generated",
"value": "15.0"
"value": "1.0"
},
"Pathfinding Y Controller/I": "0.0",
"Pathfinding Theta Controller/I": "0.0",
"Pathfinding X Controller/I": "0.0",
"Pathfinding X Controller/D": "0.0",
Expand All @@ -21,7 +21,48 @@
"comment": "Auto-Generated",
"value": "1.0"
},
"Algae Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Pathfinding Y Controller/D": "0.0",
"Processor Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Processor dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Pathfinding Theta Controller/D": "0.0",
"Coral L4 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral L3 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral proximity distance": {
"comment": "Auto-Generated",
"value": "15.0"
},
"Coral L2 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral L1 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Algae high scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Drive Speed (m/s)": {
"comment": "Auto-Generated",
"value": "4.8"
Expand All @@ -36,23 +77,22 @@
},
"Pathfinding Y Controller/P": "1.0",
"Pathfinding X Controller/P": "1.0",
"Pathfinding Theta Controller/D": "0.0",
"elevator/topPoint": {
"comment": "Auto-Generated",
"value": "1.0"
}
},
"mode": "competition",
"mode": "simulation",
"simulation": {
"elevator/bottomPoint": {
"comment": "Auto-Generated",
"value": "0.0"
},
"Coral proximity distance": {
"Pathfinding Y Controller/I": "0.0",
"Algae low scoring dist": {
"comment": "Auto-Generated",
"value": "15.0"
"value": "1.0"
},
"Pathfinding Y Controller/I": "0.0",
"Pathfinding Theta Controller/I": "0.0",
"Pathfinding X Controller/I": "0.0",
"Pathfinding X Controller/D": "0.0",
Expand All @@ -65,7 +105,48 @@
"comment": "Auto-Generated",
"value": "1.0"
},
"Algae Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Pathfinding Y Controller/D": "0.0",
"Processor Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Processor dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Pathfinding Theta Controller/D": "0.0",
"Coral L4 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral L3 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral proximity distance": {
"comment": "Auto-Generated",
"value": "15.0"
},
"Coral L2 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral L1 scoring dist": {
"comment": "Auto-Generated",
"value": "0.6"
},
"Coral Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Algae high scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Drive Speed (m/s)": {
"comment": "Auto-Generated",
"value": "4.8"
Expand All @@ -80,7 +161,6 @@
},
"Pathfinding Y Controller/P": "1.0",
"Pathfinding X Controller/P": "1.0",
"Pathfinding Theta Controller/D": "0.0",
"elevator/topPoint": {
"comment": "Auto-Generated",
"value": "1.0"
Expand All @@ -91,11 +171,11 @@
"comment": "Auto-Generated",
"value": "0.0"
},
"Coral proximity distance": {
"Pathfinding Y Controller/I": "0.0",
"Algae low scoring dist": {
"comment": "Auto-Generated",
"value": "15.0"
"value": "1.0"
},
"Pathfinding Y Controller/I": "0.0",
"Pathfinding Theta Controller/I": "0.0",
"Pathfinding X Controller/I": "0.0",
"Pathfinding X Controller/D": "0.0",
Expand All @@ -108,7 +188,48 @@
"comment": "Auto-Generated",
"value": "1.0"
},
"Algae Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Pathfinding Y Controller/D": "0.0",
"Processor Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Processor dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Pathfinding Theta Controller/D": "0.0",
"Coral L4 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral L3 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral proximity distance": {
"comment": "Auto-Generated",
"value": "15.0"
},
"Coral L2 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral L1 scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Coral Flipped": {
"comment": "Auto-Generated",
"value": "false"
},
"Algae high scoring dist": {
"comment": "Auto-Generated",
"value": "1.0"
},
"Drive Speed (m/s)": {
"comment": "Auto-Generated",
"value": "44"
Expand All @@ -123,7 +244,6 @@
},
"Pathfinding Y Controller/P": "1.0",
"Pathfinding X Controller/P": "1.0",
"Pathfinding Theta Controller/D": "0.0",
"elevator/topPoint": {
"comment": "Auto-Generated",
"value": "1.0"
Expand Down
17 changes: 17 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,19 @@

package frc.robot;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.Threads;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.util.AllianceUtil;
import frc.robot.util.AutoTargetUtils.Reef;
import frc.robot.util.AutoTargetUtils.Reef.CoralLevel;
import frc.robot.util.AutoTargetUtils.Reef.CoralReefLocation;
import me.nabdev.oxconfig.OxConfig;

import java.util.ArrayList;

import org.ironmaple.simulation.SimulatedArena;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
Expand Down Expand Up @@ -100,6 +107,7 @@ public void robotPeriodic() {
// Switch thread to high priority to improve loop timing
Threads.setCurrentThreadPriority(true, 99);

robotContainer.buttonBoard.periodic(robotContainer.drive);
robotContainer.stateMachine.periodic();

// Runs the Scheduler. This is responsible for polling buttons, adding
Expand All @@ -121,6 +129,15 @@ public void robotPeriodic() {
// PathfindingDebugUtils.drawLines("Field Map Inflated",
// DriveConstants.pathfinder.visualizeEdges(),
// DriveConstants.pathfinder.visualizeInflatedVertices());
ArrayList<Double> distances = new ArrayList<>();
for (CoralReefLocation location : Reef.CoralReefLocation.values()) {
Pose2d pose = Reef.coral(location, CoralLevel.L1);
distances.add(pose.getX());
distances.add(pose.getY());
distances.add(pose.getRotation().getDegrees());
}
double[] dist = distances.stream().mapToDouble(Double::doubleValue).toArray();
SmartDashboard.putNumberArray("Coral Reef Locations", dist);
}

/** This function is called once when the robot is disabled. */
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,8 @@
*/
public class RobotContainer {
// Subsystems
private final Drive drive;
private final ButtonBoardUtil buttonBoard;
public final Drive drive;
public final ButtonBoardUtil buttonBoard;
private final Vision vision;
private final Elevator elevator;
private final Shoulder shoulder;
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/GoToReefIntake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,12 @@
import frc.robot.subsystems.drive.DriveCommands;
import frc.robot.util.ButtonBoardUtil;

public class GoToReefIntake extends State{
public class GoToReefIntake extends State {
public GoToReefIntake(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, LEDs LEDs) {
super(stateMachine);

// TODO: rotation
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getSelectedReef(), null));
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getAlgaeReefTarget(), null));
startWhenActive(LEDs.goingToAlgaeIntake());
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/GoToScoreCoral.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,11 @@
import frc.robot.subsystems.drive.DriveCommands;
import frc.robot.util.ButtonBoardUtil;

public class GoToScoreCoral extends State{
public class GoToScoreCoral extends State {
public GoToScoreCoral(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive) {
super(stateMachine);

// TODO: change to 1-6 sides of reef
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getSelectedReef(), null));
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getCoralReefTarget(), null));
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/GoToScoreNet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,11 @@
import frc.robot.subsystems.drive.DriveCommands;
import frc.robot.util.ButtonBoardUtil;

public class GoToScoreNet extends State{
public class GoToScoreNet extends State {
public GoToScoreNet(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive) {
super(stateMachine);

// TODO: change position to net
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getSelecectedAlgae(), null));
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getAlgaeReefTarget(), null));
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/GoToScoreProcessor.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,11 @@
import frc.robot.subsystems.drive.DriveCommands;
import frc.robot.util.ButtonBoardUtil;

public class GoToScoreProcessor extends State{
public class GoToScoreProcessor extends State {
public GoToScoreProcessor(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive) {
super(stateMachine);

// TODO: change to processor
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getSelecectedAlgae(), null));
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getAlgaeReefTarget(), null));
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/GoToStationIntake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,12 @@
import frc.robot.subsystems.drive.DriveCommands;
import frc.robot.util.ButtonBoardUtil;

public class GoToStationIntake extends State{
public class GoToStationIntake extends State {
public GoToStationIntake(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, LEDs LEDs) {
super(stateMachine);

// TODO: rotation
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getSelectedStationPose(), null));
startWhenActive(DriveCommands.goToPointDesiredRot(drive, buttonBoard.getIntakeStationTarget(), null));
startWhenActive(LEDs.goingToCoralIntake());
}
}
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/IntakeAlgae.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,11 @@
import frc.robot.util.ButtonBoardUtil;

public class IntakeAlgae extends State {
public IntakeAlgae(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, Elevator elevator, EndEffector endEffector, LEDs LEDs) {
public IntakeAlgae(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, Elevator elevator,
EndEffector endEffector, LEDs LEDs) {
super(stateMachine);

startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getSelectedReef(), null));
startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getAlgaeReefTarget(), null));
startWhenActive(endEffector.runIntake());
startWhenActive(LEDs.intakingAndScoringAlgae());
}
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/statemachine/states/tele/IntakeCoral.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,11 +10,12 @@
import frc.robot.util.ButtonBoardUtil;

public class IntakeCoral extends State {
public IntakeCoral(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, Elevator elevator, EndEffector endEffector, LEDs LEDs) {
public IntakeCoral(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, Elevator elevator,
EndEffector endEffector, LEDs LEDs) {
super(stateMachine);

// TODO: Get others to make heights and (Marcus) rotation
startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getSelectedStationPose(), null));
startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getIntakeStationTarget(), null));
startWhenActive(endEffector.runIntake());
startWhenActive(LEDs.intakingAndScoringCoral());
}
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/statemachine/states/tele/ScoreAlgaeNet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,13 @@
import frc.robot.subsystems.drive.DriveCommands;
import frc.robot.util.ButtonBoardUtil;

public class ScoreAlgaeNet extends State{
public ScoreAlgaeNet(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, EndEffector endEffector, LEDs LEDs) {
public class ScoreAlgaeNet extends State {
public ScoreAlgaeNet(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive,
EndEffector endEffector, LEDs LEDs) {
super(stateMachine);

// TODO: change to net
startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getSelectedReef(), null));
startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getCoralReefTarget(), null));
startWhenActive(endEffector.runIntakeReverse());
startWhenActive(LEDs.intakingAndScoringAlgae());
}
Expand Down
Loading

0 comments on commit 64decff

Please sign in to comment.