Skip to content

Commit

Permalink
Merge pull request #28 from 2491-NoMythic/realTestSpeakerAngleMovingMath
Browse files Browse the repository at this point in the history 
fixed speakerMoving math, IT WORKS NOW. also cleaned up button bindin…
  • Loading branch information
@veggie2u
veggie2u authored Jan 27, 2024
2 parents 46aa8b6 + c89840d commit 72e6568
Show file tree
Hide file tree
Showing 4 changed files with 42 additions and 46 deletions.
42 changes: 13 additions & 29 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ public RobotContainer() {
// = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists);
driveTrainInst();
limelightInit();
autoInit();


if(intakeExists) {intakeInst();}
if(shooterExists) {shooterInst();}
Expand All @@ -129,6 +129,7 @@ public RobotContainer() {
if(lightsExist) {lightsInst();}
if(indexerExists) {indexInit();}
if(intakeExists && shooterExists && indexerExists) {indexCommandInst();}
autoInit();
// Configure the trigger bindings
configureBindings();
}
Expand Down Expand Up @@ -203,37 +204,20 @@ private void configureBindings() {
// new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain/*, shooter*/));
new Trigger(driverController::getPSButton).onTrue(new InstantCommand(driveTrain::zeroGyroscope));
SmartDashboard.putData(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle));
// new Trigger(driverController::getCrossButton).onTrue(new InstantCommand(()->SmartDashboard.putNumber("calculated robot angle", driveTrain.calculateSpeakerAngle())));
// // new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain));
// new Trigger(driverController::getCrossButton).onTrue(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle));

if(climberExists) {new Trigger(operatorController::getCrossButtonPressed).onTrue(new AutoClimb(climber));}
//Intake bindings
// new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE));
// new Trigger(operatorController::getR1Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE));
// new Trigger(operatorController::getR2Button).onTrue(new IntakeCommand(intake, iDirection.COAST));
//Path finding is pretty epic. It was a dark and stormy night. I was waiting, at my computer in the programming room.
// More specifically, I was waiting for Rowan, who was working with sequential command groups. I called over to Rowan if he could come help soon, and he answered.
//but his voice sounded kinda weird. I didn't think much of it at the time
new Trigger(driverController::getTriangleButton).onTrue(new GoToClimbSpot(driveTrain, climbSpotChooser));
new Trigger(driverController::getCrossButton).onTrue(new GoToAmp(driveTrain));

// new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain));


new Trigger(driverController::getR1Button).whileTrue(new AimRobotMoving(
driveTrain,
() -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL),
() -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
driverController::getR1Button));

// new Trigger(driverController::getCrossButton).onTrue(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle));

new Trigger(operatorController::getCircleButton).onTrue(new ManualShoot(shooter));

new Trigger(operatorController::getCrossButton).onTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
new Trigger(operatorController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop()));

if(shooterExists) {new Trigger(operatorController::getCircleButton).onTrue(new ManualShoot(shooter));}
if(climberExists) {
new Trigger(operatorController::getCrossButton).onTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
new Trigger(operatorController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop()));
new Trigger(operatorController::getCrossButtonPressed).onTrue(new AutoClimb(climber));
}
// //Intake bindings
// new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE));
// new Trigger(operatorController::getL2Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE));
Expand Down Expand Up @@ -295,11 +279,11 @@ private void configureDriveTrain() {

private void registerNamedCommands() {
NamedCommands.registerCommand("stopDrivetrain", new InstantCommand(driveTrain::stop, driveTrain));
NamedCommands.registerCommand("shooterOn", new InstantCommand(()->shooter.shootThing(1), shooter));
NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.feederFeed(0.5), indexer));
NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::feederOff, indexer));
NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));
if(intakeExists) {NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, intake));}
if(shooterExists) {NamedCommands.registerCommand("shooterOn", new InstantCommand(()->shooter.shootThing(1), shooter));}
if(indexerExists) {NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.feederFeed(0.5), indexer));
NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::feederOff, indexer));}
if(intakeExists) {NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));};
if(intakeExists) {NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, intake));}
}

public void teleopPeriodic() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/AimRobotMoving.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ public void initialize() {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
desiredRobotAngle = m_drivetrain.calculateSpeakerAngle();
desiredRobotAngle = m_drivetrain.calculateSpeakerAngleMoving();
speedController.setSetpoint(desiredRobotAngle);
//move robot to desired angle
this.currentHeading = m_drivetrain.getGyroscopeRotation().getDegrees();
Expand Down
35 changes: 22 additions & 13 deletions src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.LimelightHelpers;
import frc.robot.commands.AngleShooter;
import frc.robot.commands.Drive;
import frc.robot.commands.RotateRobot;
import frc.robot.settings.Constants;
import frc.robot.settings.LimelightValues;
Expand Down Expand Up @@ -99,14 +100,19 @@ public class DrivetrainSubsystem extends SubsystemBase {
Translation2d targetOffset;
double offsetSpeakerX;
double offsetSpeakerY;
double offsetDeltaX;
double offsetDeltaY;
Translation2d adjustedTarget;
double offsetSpeakerdist;
public double speakerDist;

Boolean lightsExist;
Limelight limelight;

double MathRanNumber;

public DrivetrainSubsystem(Lights lights, Boolean lightsExist) {
MathRanNumber = 0;
this.lights = lights;
this.lightsExist = lightsExist;

Expand Down Expand Up @@ -321,10 +327,15 @@ public double calculateSpeakerAngleMoving(){
//line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime

//next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed
offsetSpeakerX = deltaX-targetOffset.getX();
offsetSpeakerY = deltaY-targetOffset.getY();
offsetSpeakerX = Field.SPEAKER_X+targetOffset.getX();
if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) {offsetSpeakerY = Field.RED_SPEAKER_Y+targetOffset.getY();}
else {offsetSpeakerY = Field.BLUE_SPEAKER_Y+targetOffset.getY();}
offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX);
offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY);

adjustedTarget = new Translation2d(offsetSpeakerX, offsetSpeakerY);
offsetSpeakerdist = Math.sqrt(Math.pow(offsetSpeakerX, 2) + Math.pow(offsetSpeakerY, 2));
offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaY, 2) + Math.pow(offsetDeltaX, 2));
SmartDashboard.putNumber("offsetSpeakerDis", offsetSpeakerdist);
if(offsetSpeakerdist<Field.MAX_SHOOTING_DISTANCE && lightsExist) {
lights.setLights(0, Constants.LED_COUNT, 0, 100, 0);
} else {if(lightsExist) {
Expand All @@ -334,25 +345,25 @@ public double calculateSpeakerAngleMoving(){
SmartDashboard.putString("offset speaker location", new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
//getting desired robot angle
if (alliance.get() == Alliance.Blue) {
if (dtvalues.getY() >= Field.BLUE_SPEAKER_Y-targetOffset.getY()) {
double thetaAbove = -Math.toDegrees(Math.asin(deltaX / speakerDist))-90;
if (dtvalues.getY() >= adjustedTarget.getY()) {
double thetaAbove = -Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))-90;
m_desiredRobotAngle = thetaAbove;
}
else{
double thetaBelow = Math.toDegrees(Math.asin(deltaX / speakerDist))+90;
double thetaBelow = Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))+90;
m_desiredRobotAngle = thetaBelow;
} } else {
if (dtvalues.getY() >= Field.RED_SPEAKER_Y) {
double thetaAbove = -Math.toDegrees(Math.asin(deltaX / speakerDist))-90;
if (dtvalues.getY() >= adjustedTarget.getY()) {
double thetaAbove = -Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))-90;
m_desiredRobotAngle = thetaAbove;
}
else{
double thetaBelow = Math.toDegrees(Math.asin(deltaX / speakerDist))+90;
double thetaBelow = Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))+90;
m_desiredRobotAngle = thetaBelow;
}
}
SmartDashboard.putNumber("just angle to offset", Math.toDegrees(Math.asin(offsetSpeakerX / offsetSpeakerdist)));

MathRanNumber++;
SmartDashboard.putString("adjusted target", adjustedTarget.toString());
return m_desiredRobotAngle;
}
public Pose2d getAverageBotPose(LimelightValues ll2, LimelightValues ll3) {
Expand Down Expand Up @@ -394,7 +405,5 @@ public void periodic() {
//for testing RotateRobot:
SmartDashboard.putNumber("loopedHeading", getHeadingLooped());
SmartDashboard.putNumber("calculated speaker angle", calculateSpeakerAngle());

SmartDashboard.putNumber("calculated angle while moving", calculateSpeakerAngleMoving());
}
}
9 changes: 6 additions & 3 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,9 +183,12 @@ public double calculateSpeakerAngle() {
//line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime

//next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed
offsetSpeakerX = deltaX-targetOffset.getX();
offsetSpeakerY = deltaY-targetOffset.getY();
offsetSpeakerdist = Math.sqrt(Math.pow(offsetSpeakerX, 2) + Math.pow(offsetSpeakerY, 2));
offsetSpeakerX = Field.SPEAKER_X+targetOffset.getX();
if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) {offsetSpeakerY = Field.RED_SPEAKER_Y+targetOffset.getY();}
else {offsetSpeakerY = Field.BLUE_SPEAKER_Y+targetOffset.getY();}
double offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX);
double offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY);
offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2));
SmartDashboard.putString("offset amount", targetOffset.toString());
SmartDashboard.putString("offset speaker location", new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
//getting desired robot angle
Expand Down

0 comments on commit 72e6568

Please sign in to comment.