Skip to content

Commit

Permalink
Merge pull request #114 from robototes/sensors+vision
Browse files Browse the repository at this point in the history 
Changes from 4/2 testing
  • Loading branch information
@jamesdooley4
jamesdooley4 authored Apr 3, 2024
2 parents 8fdd36a + ed4032a commit b217095
Show file tree
Hide file tree
Showing 11 changed files with 126 additions and 91 deletions.
2 changes: 1 addition & 1 deletion src/main/deploy/swerve/modules/backleft.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
"i": 0,
"d": 0,
"kS": 0.18532,
"kV": 0.347198082,
"kV": 1.6909,
"kA": 0.28447
},
"angleTuning": {
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/swerve/modules/backright.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
"i": 0,
"d": 0,
"kS": 0.15508,
"kV": 0.35947706641,
"kV": 1.7507,
"kA": 0.26534
},
"angleTuning": {
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/swerve/modules/frontleft.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
"i": 0,
"d": 0,
"kS": 0.026162,
"kV": 0.3849384,
"kV": 1.8747,
"kA": 0.39611
},
"angleTuning": {
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/swerve/modules/frontright.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
"i": 0,
"d": 0,
"kS": 0.12121,
"kV": 0.368676,
"kV": 1.7955,
"kA": 0.33215
},
"angleTuning": {
Expand Down
53 changes: 3 additions & 50 deletions src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,53 +29,6 @@ public void initialize() {

@Override
public void execute() {
// intake rejecting
// if (intakeSubsystem.intakeFrontSeesNote()) {
// if (!intakeSubsystem.getRejectOverride()) {
// intakeSubsystem.intakeLeftReject();
// intakeSubsystem.intakeRightReject();
// } else {
// intakeSubsystem.intakeLeftStop();
// intakeSubsystem.intakeRightStop();
// }

// if (controls != null && !rumbledIntakeFront) {
// Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
// rumbledIntakeFront = true;
// }
// }

// // back intake sensor does not exist (but it might?)

// if (intakeSubsystem.intakeLeftSeesNote()) {
// if (!intakeSubsystem.getRejectOverride()) {
// intakeSubsystem.intakeFrontReject();
// intakeSubsystem.intakeRightReject();
// } else {
// intakeSubsystem.intakeFrontStop();
// intakeSubsystem.intakeRightStop();
// }

// if (controls != null && !rumbledIntakeLeft) {
// Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
// rumbledIntakeLeft = true;
// }
// }

// if (intakeSubsystem.intakeRightSeesNote()) {
// if (!intakeSubsystem.getRejectOverride()) {
// intakeSubsystem.intakeFrontReject();
// intakeSubsystem.intakeLeftReject();
// } else {
// intakeSubsystem.intakeFrontStop();
// intakeSubsystem.intakeLeftStop();
// }

// if (controls != null && !rumbledIntakeRight) {
// Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule();
// rumbledIntakeRight = true;
// }
// }

// all intake motors rejecting after index
if (intakeSubsystem.indexSensorHasNote()) {
Expand Down Expand Up @@ -107,9 +60,9 @@ public void end(boolean interrupted) {
Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule();
}

rumbledIntakeFront = false;
rumbledIntakeLeft = false;
rumbledIntakeRight = false;
// rumbledIntakeFront = false;
// rumbledIntakeLeft = false;
// rumbledIntakeRight = false;
rumbledIndex = false;
}

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ public void execute() {
Math.atan2(relativeSpeaker.getY(), relativeSpeaker.getX()) + Math.PI);
double distance = relativeSpeaker.getTranslation().getNorm();
LauncherDataPoint dataPoint = LAUNCHER_DATA.get(distance);
launcherSubsystem.setAngle(dataPoint.angle);
launcherSubsystem.setAngleWithOffset(dataPoint.angle);
launcherSubsystem.launch(dataPoint.rpm);
launcherSubsystem.updateDistanceEntry(distance);

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ public class DrivebaseSubsystem extends SubsystemBase {
Robot.getInstance().getRobotType() == RobotType.BONK
? 3.0
: Robot.getInstance().getRobotType() == RobotType.COMPETITION
? 6.0
? 4.7
: Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 1.0;

// Auto align stuff, dw abt it
Expand Down
109 changes: 89 additions & 20 deletions src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkLimitSwitch;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
Expand All @@ -34,6 +35,8 @@ public class IntakeSubsystem extends SubsystemBase {
public static final double FEEDER_IN_SPEED = 0.65;
public static final double FEEDER_REVERSE_SPEED = -0.3;

private static final boolean enableFrontAndSideIntakes = false;

// Motors
private final CANSparkMax intakeMotorFront;
private final CANSparkMax intakeMotorLeft;
Expand All @@ -46,9 +49,17 @@ public class IntakeSubsystem extends SubsystemBase {

// Sensors
private final SparkLimitSwitch indexSensor;
private final DigitalInput feederSensor;

private final SparkLimitSwitch feederSensor;
private final DigitalInput feederSensorIR;
private final SparkLimitSwitch intakeFrontSensor;

// debounce ! !
private final Debouncer intakeFrontSensorDebouncer;
private final Debouncer intakeRightSensorDebouncer;
private final Debouncer intakeLeftSensorDebouncer;
private final Debouncer feederSensorDebouncer;
private final Debouncer feederSensorIRDebouncer;

// private final SparkLimitSwitch intakeBackSensor;
private final SparkLimitSwitch intakeLeftSensor;
private final SparkLimitSwitch intakeRightSensor;
Expand All @@ -69,6 +80,8 @@ public class IntakeSubsystem extends SubsystemBase {
// reject override
private GenericEntry rejectOverride;

private boolean feederSensorSignal;

public IntakeSubsystem() {

intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless);
Expand All @@ -81,18 +94,22 @@ public IntakeSubsystem() {
feederMotor = new CANSparkFlex(FEEDER_MOTOR, MotorType.kBrushless);

indexSensor = indexMotorUpper.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
feederSensor = new DigitalInput(FEEDER_SENSOR);
feederSensor = feederMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
feederSensorIR = new DigitalInput(FEEDER_SENSOR);

intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakeBackSensor =
// intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);

// todo: MOVE THIS TO CONFIGURE MOTOR
intakeFrontSensor.enableLimitSwitch(false);
intakeLeftSensor.enableLimitSwitch(false);
intakeRightSensor.enableLimitSwitch(false);
// sensor must be true for 0.1 seconds before being actually true
intakeFrontSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth);
intakeRightSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth);
intakeLeftSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth);

feederSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth);
feederSensorIRDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth);

resetMotors();

Expand Down Expand Up @@ -133,9 +150,11 @@ private void resetMotors() {
}

public void intakeSet(double speed) {
intakeMotorFront.set(speed);
intakeMotorLeft.set(speed);
intakeMotorRight.set(speed);
if (enableFrontAndSideIntakes) {
intakeMotorFront.set(speed);
intakeMotorLeft.set(speed);
intakeMotorRight.set(speed);
}
ingestMotor.set(speed);
}

Expand All @@ -150,10 +169,12 @@ public void intakeReverse() {

public void intakeSteal() {

intakeMotorLeft.set(INTAKE_IN_SPEED);
intakeMotorRight.set(INTAKE_IN_SPEED);
if (enableFrontAndSideIntakes) {
intakeMotorLeft.set(INTAKE_IN_SPEED);
intakeMotorRight.set(INTAKE_IN_SPEED);
intakeMotorFront.set(INTAKE_REJECT_SPEED);
}
ingestMotor.set(INTAKE_REJECT_SPEED);
intakeMotorFront.set(INTAKE_REJECT_SPEED);
indexMotorUpper.set(INTAKE_IN_SPEED);
}

Expand All @@ -163,15 +184,21 @@ public void intakeStop() {
}

public void intakeFrontStop() {
intakeMotorFront.set(0);
if (enableFrontAndSideIntakes) {
intakeMotorFront.set(0);
}
}

public void intakeLeftStop() {
intakeMotorLeft.set(0);
if (enableFrontAndSideIntakes) {
intakeMotorLeft.set(0);
}
}

public void intakeRightStop() {
intakeMotorRight.set(0);
if (enableFrontAndSideIntakes) {
intakeMotorRight.set(0);
}
}

// intake reject methods
Expand All @@ -180,15 +207,21 @@ public void intakeReject() {
}

public void intakeFrontReject() {
intakeMotorFront.set(INTAKE_REJECT_SPEED);
if (enableFrontAndSideIntakes) {
intakeMotorFront.set(INTAKE_REJECT_SPEED);
}
}

public void intakeLeftReject() {
intakeMotorLeft.set(INTAKE_REJECT_SPEED);
if (enableFrontAndSideIntakes) {
intakeMotorLeft.set(INTAKE_REJECT_SPEED);
}
}

public void intakeRightReject() {
intakeMotorRight.set(INTAKE_REJECT_SPEED);
if (enableFrontAndSideIntakes) {
intakeMotorRight.set(INTAKE_REJECT_SPEED);
}
}

// index methods
Expand Down Expand Up @@ -227,8 +260,17 @@ public boolean indexSensorHasNote() {
return indexSensor.isPressed();
}

public boolean debouncedFeederSensor() {
// return feederSensorDebouncer.calculate(feederSensor.isPressed());

boolean feederSensorSignal = feederSensorDebouncer.calculate(feederSensor.isPressed());
boolean feederSensorIRSignal = feederSensorIRDebouncer.calculate(!feederSensorIR.get());

return feederSensorSignal || feederSensorIRSignal;
}

public boolean feederSensorHasNote() {
return !feederSensor.get() && !getSensorOverride();
return debouncedFeederSensor() && !getSensorOverride();
}

public boolean intakeFrontSeesNote() {
Expand All @@ -243,6 +285,28 @@ public boolean intakeRightSeesNote() {
return intakeRightSensor.isPressed();
}

// debounce sensors
public boolean debouncedIntakeFrontSensor() {
// if (intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed())) {
// return true;
// }
return intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed());
}

public boolean debouncedIntakeLeftSensor() {
// if (intakeLeftSensorDebouncer.calculate(intakeLeftSensor.isPressed())) {
// return true;
// }
return intakeLeftSensorDebouncer.calculate(intakeFrontSensor.isPressed());
}

public boolean debouncedIntakeRightSensor() {
// if (intakeRightSensorDebouncer.calculate(intakeRightSensor.isPressed())) {
// return true;
// }
return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed());
}

// override methods on shuffleboard
public boolean getSensorOverride() {
return sensorOverride.getBoolean(false);
Expand Down Expand Up @@ -298,6 +362,11 @@ public void initShuffleboard() {
shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1);
shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1);

shuffleboardTab.addBoolean(
"Feeder beambreak", () -> feederSensorDebouncer.calculate(feederSensor.isPressed()));
shuffleboardTab.addBoolean(
"Feeder IR", () -> feederSensorIRDebouncer.calculate(!feederSensorIR.get()));

// no intake back sensor
shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1);
shuffleboardTab.addBoolean("Intake Left Sensor - ", this::intakeLeftSeesNote).withSize(1, 1);
Expand Down
Loading

0 comments on commit b217095

Please sign in to comment.