Skip to content

Commit

Permalink
Merge branch 'master' into fix-line-endings
Browse files Browse the repository at this point in the history
  • Loading branch information
brettle authored Apr 24, 2024
2 parents 912e8d0 + b3acee2 commit e42c032
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 89 deletions.
4 changes: 3 additions & 1 deletion src/main/java/org/carlmontrobotics/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -373,7 +373,9 @@ public static final class Limelightc {
public static final class Apriltag {
public static final int RED_SPEAKER_CENTER_TAG_ID = 4;
public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7;
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); // 88.125
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125
public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26);
public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875);
}
}

Expand Down
43 changes: 0 additions & 43 deletions src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java

This file was deleted.

43 changes: 0 additions & 43 deletions src/main/java/org/carlmontrobotics/commands/AimAtSpeaker.java

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@
package org.carlmontrobotics.commands;

import static org.carlmontrobotics.Constants.Limelightc.*;

import org.carlmontrobotics.Constants.Limelightc;

import static org.carlmontrobotics.Constants.Effectorc.*;

import org.carlmontrobotics.subsystems.Drivetrain;
Expand Down Expand Up @@ -47,8 +50,8 @@ public void initialize() {

@Override
public void execute() {
double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(INTAKE_LL_NAME));
double forwardDistErrMeters = ll.getDistanceToNoteMeters();
double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME));
double forwardDistErrMeters = ll.getDistanceToNoteMeters();
double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad);
// dt.drive(0,0,0);
dt.drive(Math.max(forwardDistErrMeters * 2, MIN_MOVEMENT_METERSPSEC),
Expand Down
59 changes: 59 additions & 0 deletions src/main/java/org/carlmontrobotics/commands/MoveToNote.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.commands;

import org.carlmontrobotics.Constants.Limelightc;
import org.carlmontrobotics.subsystems.Drivetrain;
import org.carlmontrobotics.subsystems.Limelight;
import org.carlmontrobotics.subsystems.LimelightHelpers;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;

public class MoveToNote extends Command {
private final Drivetrain dt;
private final Limelight ll;
private Timer timer = new Timer();
private boolean originalFieldOrientation;
/** Creates a new MoveToNote. */
public MoveToNote(Drivetrain dt, Limelight ll) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(this.dt=dt);
this.ll = ll;
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
originalFieldOrientation = dt.getFieldOriented();
timer.reset();
timer.start();
dt.setFieldOriented(false);
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double radErr = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME));
double distErr = ll.getDistanceToNoteMeters(); //meters
double forwardErr = distErr * Math.cos(radErr);
dt.drive(Math.max(forwardErr*2, .5), 0, 0);
//180deg is about 6.2 rad/sec, min is .5rad/sec
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
dt.setFieldOriented(originalFieldOrientation);
}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() >= 0.5;
}
}

0 comments on commit e42c032

Please sign in to comment.