Skip to content

Commit

Permalink
Final Commit, remove unused shooter code
Browse files Browse the repository at this point in the history
  • Loading branch information
MichaelLesirge committed Jun 13, 2024
1 parent cf3caea commit 09e14d3
Showing 1 changed file with 2 additions and 20 deletions.
22 changes: 2 additions & 20 deletions src/main/java/frc/robot/subsystems/intake/RealShooter.java
Original file line number Diff line number Diff line change
@@ -1,13 +1,9 @@
package frc.robot.subsystems.intake;

import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.playingwithfusion.TimeOfFlight;
import com.playingwithfusion.TimeOfFlight.RangingMode;
import com.revrobotics.CANSparkLowLevel;
import com.revrobotics.CANSparkMax;

import edu.wpi.first.math.filter.MedianFilter;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Constants.IntakeShooterConstants;

Expand All @@ -27,27 +23,15 @@ public class RealShooter extends IntakeShooter {
* 2 for intake
* neo550s, 2 sim motors,
*/

static private final double DISTANCE_OF_NOTE_METERS = Units.inchesToMeters(3);

private final TimeOfFlight lidar;
private final MedianFilter sensorMedianFilter = new MedianFilter(5);

private final WPI_VictorSPX flywheel1;
private final WPI_VictorSPX flywheel2;

private final CANSparkMax intake;

// private final DigitalInput intakeSwitch;

public RealShooter(int flywheel1Id, int flywheel2Id, int intakeID, int intakeLimitSwitchId, int lidarId) {
this.flywheel1 = new WPI_VictorSPX(flywheel1Id);
this.flywheel2 = new WPI_VictorSPX(flywheel2Id);

this.lidar = new TimeOfFlight(lidarId);
lidar.setRangingMode(RangingMode.Short, 25);
// this.intakeSwitch = new DigitalInput(intakeLimitSwitchId);

this.intake = new CANSparkMax(intakeID, CANSparkLowLevel.MotorType.kBrushless);
intake.setInverted(IntakeShooterConstants.INTAKE_REVERSE);
}
Expand All @@ -69,8 +53,7 @@ public void setIntakeGrabberSpeed(double speed) {

@Override
public boolean hasNoteInIntake() {
// return intakeSwitch.get();
return getLidar() < DISTANCE_OF_NOTE_METERS;
return false;
}

@Override
Expand All @@ -89,8 +72,7 @@ public void stop() {
}

public double getLidar() {
return sensorMedianFilter.calculate(
lidar.getRange() / 1000);
return 0;
}

@Override
Expand Down

0 comments on commit 09e14d3

Please sign in to comment.