Skip to content

Commit

Permalink
shooter pivots whole time
Browse files Browse the repository at this point in the history
  • Loading branch information
@CrolineCrois
CrolineCrois committed Apr 6, 2024
1 parent 451379c commit c7f7dfc
Show file tree
Hide file tree
Showing 6 changed files with 217 additions and 16 deletions.
202 changes: 202 additions & 0 deletions src/main/deploy/choreo/14.traj
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,202 @@
{
"samples": [
{
"x": 2.9,
"y": 6.113,
"heading": 0.2425,
"angularVelocity": 0,
"velocityX": -1.7262570035586016e-28,
"velocityY": 1.362097567492257e-31,
"timestamp": 0
},
{
"x": 2.938903531068027,
"y": 6.123629827485452,
"heading": 0.2420519032802004,
"angularVelocity": -0.00453932999729365,
"velocityX": 0.3941023394250247,
"velocityY": 0.10768276721142236,
"timestamp": 0.09871428605266704
},
{
"x": 3.016710592745661,
"y": 6.144889482331082,
"heading": 0.2411557122702183,
"angularVelocity": -0.009078635381911062,
"velocityX": 0.7882046742061307,
"velocityY": 0.2153655331538105,
"timestamp": 0.19742857210533407
},
{
"x": 3.1334211844598885,
"y": 6.176778964380304,
"heading": 0.23981143000413516,
"angularVelocity": -0.013617910030465686,
"velocityX": 1.1823070031824825,
"velocityY": 0.3230482975099222,
"timestamp": 0.2961428581580011
},
{
"x": 3.2890353054740027,
"y": 6.219298273431795,
"heading": 0.2380190603780691,
"angularVelocity": -0.01815714521029317,
"velocityX": 1.5764093246958044,
"velocityY": 0.430731059826591,
"timestamp": 0.39485714421066814
},
{
"x": 3.4835529548057638,
"y": 6.272447409217136,
"heading": 0.23577860857806224,
"angularVelocity": -0.02269632785411698,
"velocityX": 1.9705116362588007,
"velocityY": 0.5384138194241223,
"timestamp": 0.49357143026333516
},
{
"x": 3.7169741310801077,
"y": 6.336226371360569,
"heading": 0.23309008184694724,
"angularVelocity": -0.02723543712609413,
"velocityX": 2.364613933892051,
"velocityY": 0.6460965752151011,
"timestamp": 0.5922857163160022
},
{
"x": 3.989298832234584,
"y": 6.4106351592985,
"heading": 0.2299534910085373,
"angularVelocity": -0.031774436749927436,
"velocityX": 2.7587162106321825,
"velocityY": 0.7537793252967542,
"timestamp": 0.6910000023686693
},
{
"x": 4.300527054832155,
"y": 6.4956737720917275,
"heading": 0.22636885398872517,
"angularVelocity": -0.03631325477963944,
"velocityX": 3.1528184525542584,
"velocityY": 0.8614620658640751,
"timestamp": 0.7897142884213364
},
{
"x": 4.65065879200003,
"y": 6.591342207862238,
"heading": 0.22233620618042166,
"angularVelocity": -0.04085171426963709,
"velocityX": 3.546920624853299,
"velocityY": 0.969144787406631,
"timestamp": 0.8884285744740035
},
{
"x": 5.039694023127982,
"y": 6.697640460978281,
"heading": 0.21785564962207069,
"angularVelocity": -0.04538914008275375,
"velocityX": 3.9410225883656986,
"velocityY": 1.0768274518982093,
"timestamp": 0.9871428605266706
},
{
"x": 5.428729254257294,
"y": 6.803938714091145,
"heading": 0.21337509407175914,
"angularVelocity": -0.04538912987106772,
"velocityX": 3.9410225883794716,
"velocityY": 1.0768274518659884,
"timestamp": 1.0858571465793376
},
{
"x": 5.778860991426467,
"y": 6.899607149858598,
"heading": 0.20934244723245,
"angularVelocity": -0.04085170445348799,
"velocityX": 3.5469206248664467,
"velocityY": 0.9691447873756708,
"timestamp": 1.1845714326320047
},
{
"x": 6.090089214025261,
"y": 6.984645762648941,
"heading": 0.20575781112613004,
"angularVelocity": -0.036313245525740584,
"velocityX": 3.1528184525666556,
"velocityY": 0.8614620658348401,
"timestamp": 1.2832857186846718
},
{
"x": 6.3624139151808645,
"y": 7.059054550584212,
"heading": 0.2026212211265867,
"angularVelocity": -0.03177442825200412,
"velocityX": 2.7587162106435894,
"velocityY": 0.7537793252698134,
"timestamp": 1.3820000047373389
},
{
"x": 6.595835091456214,
"y": 7.122833512725263,
"heading": 0.19993269514249148,
"angularVelocity": -0.027235429558601355,
"velocityX": 2.364613933902241,
"velocityY": 0.646096575190986,
"timestamp": 1.480714290790006
},
{
"x": 6.790352740788842,
"y": 7.17598264850855,
"heading": 0.19769224398322777,
"angularVelocity": -0.02269632136323262,
"velocityX": 1.9705116362675763,
"velocityY": 0.5384138194032987,
"timestamp": 1.579428576842673
},
{
"x": 6.945966861803667,
"y": 7.218501957558349,
"heading": 0.19589987488023405,
"angularVelocity": -0.01815713991144325,
"velocityX": 1.5764093247030033,
"velocityY": 0.43073105980945287,
"timestamp": 1.67814286289534
},
{
"x": 7.062677453518438,
"y": 7.2503914396062745,
"heading": 0.19455559301125175,
"angularVelocity": -0.013617906007737414,
"velocityX": 1.1823070031879788,
"velocityY": 0.32304829749678704,
"timestamp": 1.7768571489480072
},
{
"x": 7.140484515196437,
"y": 7.271651094451027,
"heading": 0.19365940226718134,
"angularVelocity": -0.009078632688161059,
"velocityX": 0.7882046742098351,
"velocityY": 0.21536553314491833,
"timestamp": 1.8755714350006742
},
{
"x": 7.179388046264648,
"y": 7.282280921936035,
"heading": 0.1932113056799297,
"angularVelocity": -0.004539328654550442,
"velocityX": 0.39410233942688466,
"velocityY": 0.10768276720693493,
"timestamp": 1.9742857210533413
},
{
"x": 7.179388046264648,
"y": 7.282280921936035,
"heading": 0.1932113056799297,
"angularVelocity": 0,
"velocityX": -3.174166790829149e-31,
"velocityY": 0,
"timestamp": 2.073000007106008
}
]
}
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import static frc.robot.Constants.VisionConstants.BACK_RIGHT_CAMERA;
import static frc.robot.Constants.VisionConstants.NOTE_CAMERA;

import java.util.function.BooleanSupplier;

import com.choreo.lib.ChoreoTrajectory;
import edu.wpi.first.cscore.MjpegServer;
Expand Down Expand Up @@ -592,6 +591,6 @@ private void configureBindings() {
* @return The selected autonomous command.
*/
public Command getAutonomousCommand() {
return autonBuilder.getMiddleTwoPiece();//autonPathChooser.getSelected();
return autonBuilder.getMiddleTwoPieceThen2TopCenter();//autonPathChooser.getSelected();
}
}
11 changes: 8 additions & 3 deletions src/main/java/frc/robot/commands/auton/AutonBuilder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,8 +139,7 @@ public Command goIntake(ChoreoTrajectory intakeTrajectory) {
* @return shoot command
*/
public Command shoot() {
return new ShooterPivotAimCommand(shooterPivotSubsystem)
.alongWith(new SetCalculatedAngleCommand(swerveSubsystem)).withTimeout(1)
return new SetCalculatedAngleCommand(swerveSubsystem).withTimeout(1)
.andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem).until(
() -> !intakeRollerSubsystem.getRockwellSensorValue())
.andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem)).withTimeout(.5)
Expand Down Expand Up @@ -186,7 +185,13 @@ public SequentialCommandGroup buildAuton(Pose2d initPose, Command... commands) {
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1)
).withTimeout(2)
);
autonSequence.addCommands(commands);
autonSequence.addCommands(
(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem, lightBarSubsystem)).alongWith(
new ShooterPivotAimCommand(shooterPivotSubsystem),
new SequentialCommandGroup(commands)
)
// commands
);
autonSequence.addCommands(
new WaitCommand(2),
new ShooterFlywheelStopCommand(shooterFlywheelSubsystem)
Expand Down
5 changes: 0 additions & 5 deletions src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,4 @@ public void execute() {

lightBarSubsystem.updateShooterSpeedPercentage(avg);
}

@Override
public boolean isFinished() {
return shooterSubsystem.atSpeed();
}
}
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +26,9 @@ public void end(boolean interrupted) {
shooterPivotSubsystem.setAutoAimBoolean(false);
}

@Override
public boolean isFinished() {
return Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle())
< ShooterConstants.PID_ERROR_TOLERANCE;
}
// @Override
// public boolean isFinished() {
// return Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle())
// < ShooterConstants.PID_ERROR_TOLERANCE;
// }
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,6 +177,6 @@ public void periodic() {
lightBarSubsystem.setLightBarStatus(LightBarStatus.HOLDING_NOTE, 2);
}

System.out.println(getAmpSensor());
// System.out.println(getAmpSensor());
}
}

0 comments on commit c7f7dfc

Please sign in to comment.