Skip to content

Commit

Permalink
Add intake limit switch
Browse files Browse the repository at this point in the history
  • Loading branch information
@Blue-Flag-666
Blue-Flag-666 committed Aug 13, 2024
1 parent 081b25f commit 141be9a
Show file tree
Hide file tree
Showing 6 changed files with 56 additions and 43 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/libzodiac
Submodule libzodiac updated 5 files
+2 −3 Constant.java
+2 −2 Zwerve.java
+5 −0 hardware/Falcon.java
+3 −3 hardware/group/FalconSwerve.java
+6 −6 hardware/group/FalconWithEncoder.java
12 changes: 8 additions & 4 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ public void robotInit() {
// and put our
// autonomous chooser on the dashboard.
m_bot = new RobotContainer().init();
//m_bot.chassis.clear();
m_bot.intake.lift.reset();
}

/**
Expand Down Expand Up @@ -82,7 +84,7 @@ public void autonomousInit() {
// }
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
m_bot.chassis.clear();
//m_bot.chassis.clear();
}

/**
Expand All @@ -109,6 +111,7 @@ public void teleopInit() {
// if (RobotContainer.getTeleopIntakeCommand() != null)
// RobotContainer.getTeleopIntakeCommand().schedule();
m_bot.chassis.reset();
m_bot.intake.lift.reset();
}

/**
Expand All @@ -117,9 +120,9 @@ public void teleopInit() {
@Override
public void teleopPeriodic() {
final var scheduler = CommandScheduler.getInstance();
// scheduler.schedule(m_bot.chassis.drive(m_bot.drive));
// scheduler.run();
// scheduler.schedule(m_bot.chassis.check_wheel_reset(m_bot.drive));
scheduler.schedule(m_bot.chassis.drive(m_bot.drive));
scheduler.run();
scheduler.schedule(m_bot.chassis.check_wheel_reset(m_bot.drive));
scheduler.run();
}

Expand All @@ -128,6 +131,7 @@ public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
m_bot.chassis.clear();
m_bot.chassis.reset();
}

/**
Expand Down
17 changes: 9 additions & 8 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,14 +74,15 @@ public RobotContainer init() {
*/
private void configureBindings() {
final var ctrl = new CommandXboxController(0);
ctrl.a().toggleOnTrue(new Zambda<>(Intake::drop, this.intake));
ctrl.a().toggleOnFalse(new Zambda<>(Intake::standby, this.intake));
ctrl.leftTrigger().toggleOnTrue(new Zambda<>(Intake::take, this.intake));
ctrl.leftTrigger().toggleOnFalse(new Zambda<>(Intake::standby, this.intake));
ctrl.rightTrigger().toggleOnTrue(new Zambda<>(Shooter::shoot, this.shooter));
ctrl.rightTrigger().toggleOnTrue(new Zambda<>(Intake::send, this.intake));
ctrl.rightTrigger().toggleOnFalse(new Zambda<>(Intake::standby, this.intake));
ctrl.rightTrigger().toggleOnFalse(new Zambda<>(Shooter::standby, this.shooter));
// ctrl.a().onTrue(new Zambda<>(Intake::drop, this.intake));
// ctrl.a().onFalse(new Zambda<>(Intake::standby, this.intake));
// todo :change the key
ctrl.leftTrigger().onTrue(new Zambda<>(Intake::take, this.intake));
ctrl.leftTrigger().onFalse(new Zambda<>(Intake::standby, this.intake));
ctrl.rightTrigger().onTrue(new Zambda<>(Shooter::shoot, this.shooter));
ctrl.rightTrigger().onTrue(new Zambda<>(Intake::send, this.intake));
ctrl.rightTrigger().onFalse(new Zambda<>(Intake::standby, this.intake));
ctrl.rightTrigger().onFalse(new Zambda<>(Shooter::standby, this.shooter));
}

}
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/Chassis.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,10 @@ public class Chassis extends Zwerve {
};

private static final FalconSwerve[] mods = {
new FalconSwerve(speed[0], angle[0], new Vec2D(1, 1).mul(0.25)),
new FalconSwerve(speed[1], angle[1], new Vec2D(1, -1).mul(0.25)),
new FalconSwerve(speed[2], angle[2], new Vec2D(-1, 1).mul(0.25)),
new FalconSwerve(speed[3], angle[3], new Vec2D(1, -1).mul(0.25)),
new FalconSwerve(speed[0], angle[0], new Vec2D(1, 1).mul(1)),
new FalconSwerve(speed[1], angle[1], new Vec2D(1, -1).mul(1)),
new FalconSwerve(speed[2], angle[2], new Vec2D(-1, 1).mul(1)),
new FalconSwerve(speed[3], angle[3], new Vec2D(1, -1).mul(1)),
};

private static final Pigeon gyro = new Pigeon(0);
Expand All @@ -58,7 +58,6 @@ public class Chassis extends Zwerve {
*/
public Chassis() {
super(mods, gyro, new Vec2D(114, 114));
super.output = 0.001;
var v = new PIDProfile(0.15, 0, 0);
var a = new PIDProfile(0.05, 0.001, 0);
// Mod I.
Expand Down
40 changes: 24 additions & 16 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,18 @@
package frc.robot.subsystems;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.libzodiac.hardware.Falcon;
import frc.libzodiac.hardware.Pro775;
import frc.libzodiac.ZMotor;
import frc.libzodiac.ZmartDash;
import frc.libzodiac.hardware.Falcon;
import frc.libzodiac.hardware.Pro775;

public final class Intake extends SubsystemBase implements ZmartDash {
public final ZMotor convey = new Falcon(31);
public final Pro775.Servo lift = new Pro775.Servo(32);
private final DigitalInput toplimitSwitch = new DigitalInput(0);
private final DigitalInput bottomlimitSwitch = new DigitalInput(1);
private State state = State.Standby;

public Intake() {
this.convey.set_pid(0.01, 0, 0);
Expand All @@ -26,20 +30,13 @@ public Intake init() {
return this;
}

private enum State {
Standby,
Taking,
Sending,
Dropping,
}

private State state = State.Standby;

public Intake standby() {
// if (this.state == State.Standby)
// return this;
this.state = State.Standby;
this.lift.go("standby");
if ((this.lift.get() < this.lift.profile.get("standby") & this.bottomlimitSwitch.get()) || (this.lift.get() > this.lift.profile.get("standby") & this.toplimitSwitch.get())) {
this.lift.go("standby");
}
this.convey.shutdown();
this.debug("state", "standby");
return this;
Expand All @@ -49,7 +46,9 @@ public Intake take() {
// if (this.state == State.Taking)
// return this;
this.state = State.Taking;
this.lift.go("down");
if (this.bottomlimitSwitch.get()) {
this.lift.go("down");
}
this.convey.go("in");
this.debug("state", "taking");
return this;
Expand All @@ -59,8 +58,14 @@ public Intake send() {
// if (this.state == State.Sending)
// return this;
this.state = State.Sending;
this.lift.go("up");
this.convey.go("out");
if (this.toplimitSwitch.get()) {
this.lift.go("up");
}
if (!this.toplimitSwitch.get()) {
this.convey.go("out");
} else {
this.convey.shutdown();
}
this.debug("state", "sending");
return this;
}
Expand All @@ -77,11 +82,14 @@ public Intake drop() {

@Override
public void periodic() {
return;
}

@Override
public String key() {
return "Intake";
}

private enum State {
Standby, Taking, Sending, Dropping,
}
}
19 changes: 10 additions & 9 deletions src/main/java/frc/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ public final class Shooter extends SubsystemBase implements ZmartDash {

private final Falcon down = new Falcon(21);
private final Falcon up = new Falcon(22);
private State state = State.Standby;

public Shooter() {
}
Expand All @@ -18,13 +19,6 @@ public Shooter init() {
return this;
}

private enum State {
Standby,
Shooting,
}

private State state = State.Standby;

public Shooter standby() {
// if (this.state == State.Standby)
// return this;
Expand All @@ -40,13 +34,20 @@ public Shooter shoot() {
// return this;
this.state = State.Shooting;
this.debug("state", "shooting");
this.down.go(-30);
this.up.go(30);
this.down.set(-0.8);
this.up.set(0.8);
//this.down.go(-100);
//this.up.go(100);
return this;
}

@Override
public String key() {
return "Shooter";
}

private enum State {
Standby,
Shooting,
}
}

0 comments on commit 141be9a

Please sign in to comment.