Skip to content
This repository has been archived by the owner on Jan 16, 2024. It is now read-only.

Commit

Permalink
feat: what's sleep??????
Browse files Browse the repository at this point in the history
  • Loading branch information
mimizh2418 committed Dec 15, 2023
1 parent ca0907c commit 2eaa1ef
Show file tree
Hide file tree
Showing 11 changed files with 205 additions and 39 deletions.
15 changes: 11 additions & 4 deletions src/main/java/org/team1540/bunnybotTank2023/Constants.java
Original file line number Diff line number Diff line change
@@ -1,10 +1,11 @@
package org.team1540.bunnybotTank2023;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;

public final class Constants {
// Allow PID constants to be tuned from a dashboard input or not
public static final boolean TUNING_MODE = true;
public static final boolean TUNING_MODE = false;

// Simulation mode, irrelevant for code running on physical robot
public static final SimulationMode simulationMode = SimulationMode.SIM;
Expand Down Expand Up @@ -76,10 +77,16 @@ public static class ShooterConstants {
}

public static class TurretConstants{
public static final int MOTOR_ID = 9; //TODO: MAKE SURE THIS CAN ID WORKS!!!
public static final double kP = 3.2;
public static final int MOTOR_ID = 10; //TODO: MAKE SURE THIS CAN ID WORKS!!!
public static final double kP = 3.636;
public static final double kI = 0;
public static final double kD = 0.1;
public static final double kD = 0;
public static final double gearRatio = 166.66666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666667;

public static final double CRUISE_VELOCITY_RPS = 100;
public static final double MAX_ACCEL_RPS2 = 600;

public static final Rotation2d FORWARD_LIMIT_POSITION = Rotation2d.fromDegrees(66.064);
public static final Rotation2d REVERSE_LIMIT_POSITION = Rotation2d.fromDegrees(150);
}
}
34 changes: 22 additions & 12 deletions src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,28 +5,31 @@

package org.team1540.bunnybotTank2023;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes;
import org.team1540.bunnybotTank2023.commands.drivetrain.ArcadeDriveCommand;
import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain;
import org.team1540.bunnybotTank2023.commands.indexer.Indexer;
import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand;
import org.team1540.bunnybotTank2023.commands.indexer.IndexerIdleCommand;
import org.team1540.bunnybotTank2023.commands.intake.Intake;
import org.team1540.bunnybotTank2023.commands.intake.IntakeCommand;
import org.team1540.bunnybotTank2023.commands.shooter.Shooter;
import org.team1540.bunnybotTank2023.commands.turret.Turret;
import org.team1540.bunnybotTank2023.commands.turret.TurretManualCommand;
import org.team1540.bunnybotTank2023.commands.turret.TurretSetpointCommand;
import org.team1540.bunnybotTank2023.commands.turret.TurretZeroSequenceCommand;
import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOSim;
import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOReal;
import org.team1540.bunnybotTank2023.io.indexer.IndexerIOReal;
import org.team1540.bunnybotTank2023.io.intake.IntakeIOReal;
import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal;
import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim;

import static org.team1540.bunnybotTank2023.Constants.*;
import org.team1540.bunnybotTank2023.io.turret.TurretIOReal;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -40,12 +43,13 @@ public class RobotContainer {
Shooter shooter;
Indexer indexer;
Intake intake;
Turret turret;

// Controllers
CommandXboxController driver = new CommandXboxController(0);
CommandXboxController copilot = new CommandXboxController(1);

LoggedDashboardNumber shooterSpeed = new LoggedDashboardNumber("Shooter/setpoint");
LoggedDashboardNumber turretSetpoint = new LoggedDashboardNumber("Turret/setpoint");

public RobotContainer() {
if (Robot.isReal()) {
Expand All @@ -54,12 +58,14 @@ public RobotContainer() {
shooter = new Shooter(new ShooterIOReal());
indexer = new Indexer(new IndexerIOReal());
intake = new Intake(new IntakeIOReal());
turret = new Turret(new TurretIOReal());
} else {
// Initialize subsystems with simulation IO
drivetrain = new Drivetrain(new DrivetrainIOSim());
shooter = new Shooter(new ShooterIOSim());
indexer = null;
intake = null;
turret = null;
}
setDefaultCommands();
configureButtonBindings();
Expand All @@ -69,7 +75,7 @@ public RobotContainer() {
/** Use this method to define your trigger->command mappings. */
private void configureButtonBindings() {
copilot.a()
.whileTrue(new InstantCommand(() -> shooter.setVelocity(shooterSpeed.get()))
.whileTrue(new InstantCommand(() -> shooter.setVelocity(turretSetpoint.get()))
.andThen(new IndexerCommand(indexer)))
.onFalse(new InstantCommand(() -> {
shooter.stop();
Expand All @@ -78,17 +84,21 @@ private void configureButtonBindings() {

// copilot.b().whileTrue(new InstantCommand(() -> intake.setFold(false)).andThen(new IndexerCommand(indexer))).whileFalse(new InstantCommand(() -> intake.setFold(true)));
copilot.b().whileTrue(Commands.parallel(new IntakeCommand(intake), new IndexerCommand(indexer)));
copilot.x().whileTrue(new TurretSetpointCommand(turret, Rotation2d.fromDegrees(turretSetpoint.get())));
copilot.y().whileTrue(new TurretZeroSequenceCommand(turret));

// copilot.x().onTrue(new InstantCommand(() -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true)));
}

private void setDefaultCommands() {
drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver));
shooter.setDefaultCommand(new StartEndCommand(
() -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
() -> {},
shooter
));
// drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver));
// shooter.setDefaultCommand(new StartEndCommand(
// () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
// () -> {},
// shooter
// ));
turret.setDefaultCommand(new TurretManualCommand(turret, copilot));
// indexer.setDefaultCommand(new IndexerIdleCommand(indexer));
}


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ public class IndexerCommand extends CommandBase {

public IndexerCommand(Indexer indexer) {
this.indexer = indexer;
addRequirements(indexer);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package org.team1540.bunnybotTank2023.commands.indexer;

import edu.wpi.first.wpilibj2.command.CommandBase;

public class IndexerIdleCommand extends CommandBase {
private final Indexer indexer;

public IndexerIdleCommand(Indexer indexer) {
this.indexer = indexer;
addRequirements(indexer);
}

@Override
public void initialize() {
indexer.setBottomSpeed(0.4);
indexer.setTopSpeed(-0.2);
}
}

Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@ public void periodic() {
}

public void setVelocity(double speedRPM) {
System.out.println("test");
setpoint = speedRPM;
averageFilter.clear();
io.setVelocity(speedRPM);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,25 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
import org.team1540.bunnybotTank2023.Constants;
import org.team1540.bunnybotTank2023.io.turret.TurretInputsAutoLogged;
import org.team1540.bunnybotTank2023.io.turret.TurretIO;
import org.team1540.bunnybotTank2023.utils.AverageFilter;
import org.team1540.bunnybotTank2023.utils.LoggedTunableNumber;

import static org.team1540.bunnybotTank2023.Constants.*;

public class Turret extends SubsystemBase {
//fields :)
private final TurretInputsAutoLogged inputs = new TurretInputsAutoLogged();
private final TurretIO io;

private final AverageFilter averageFilter = new AverageFilter(20);

private final LoggedTunableNumber kP = new LoggedTunableNumber("Turret/kP", TurretConstants.kP);
private final LoggedTunableNumber kI = new LoggedTunableNumber("Turret/kI", TurretConstants.kI);
private final LoggedTunableNumber kD = new LoggedTunableNumber("Turret/kD", TurretConstants.kD);

//constructor
public Turret(TurretIO io){
this.io = io;
Expand All @@ -20,6 +31,11 @@ public Turret(TurretIO io){
public void periodic(){
io.updateInputs(inputs);
Logger.getInstance().processInputs("Turret", inputs);
averageFilter.add(inputs.turretCurrentPositionDegrees);

if (TUNING_MODE && (kP.hasChanged() || kI.hasChanged() || kD.hasChanged())) {
io.configurePID(kP.get(), kI.get(), kD.get());
}
}

public boolean getForwardLimitSwitch(){
Expand All @@ -30,14 +46,27 @@ public boolean getReverseLimitSwitch(){
return inputs.reverseLimitSwitch;
}

public boolean isAtSetpoint() {
return Math.abs(inputs.turretSetPointDegrees - averageFilter.getAverage()) < 0.5;
}

public void resetToEncoder(Rotation2d position) {
io.resetEncoder(position);
}

public void autoSetPosition(Rotation2d position){
averageFilter.clear();
io.setTurretPosition(position);
}

public void setVoltage(double volts){
io.setVoltage(volts);
}

public void stop() {
setVoltage(0);
}

}


Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
package org.team1540.bunnybotTank2023.commands.turret;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.team1540.bunnybotTank2023.Constants;

public class TurretManualCommand extends CommandBase {
private final Turret turret;
private final CommandXboxController controller;

public TurretManualCommand(Turret turret, CommandXboxController controller) {
this.turret = turret;
this.controller = controller;
addRequirements(turret);
}

@Override
public void execute() {
turret.setVoltage(MathUtil.applyDeadband(controller.getRightX(), Constants.DEADZONE_RADIUS) * 12.0);
}

@Override
public void end(boolean interrupted) {
turret.stop();
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
package org.team1540.bunnybotTank2023.commands.turret;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;

public class TurretSetpointCommand extends CommandBase {
private final Turret turret;
private final Rotation2d setpoint;

public TurretSetpointCommand(Turret turret, Rotation2d setpoint) {
this.turret = turret;
this.setpoint = setpoint;
addRequirements(turret);
}

@Override
public void initialize() {
turret.autoSetPosition(setpoint);
}

@Override
public boolean isFinished() {
return turret.isAtSetpoint();
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
package org.team1540.bunnybotTank2023.commands.turret;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;

import static org.team1540.bunnybotTank2023.Constants.*;

public class TurretZeroSequenceCommand extends SequentialCommandGroup {
public TurretZeroSequenceCommand(Turret turret) {
addCommands(
new InstantCommand(() -> turret.setVoltage(2)),
new WaitUntilCommand(turret::getForwardLimitSwitch),
new InstantCommand(() -> turret.resetToEncoder(TurretConstants.FORWARD_LIMIT_POSITION)),
new TurretSetpointCommand(turret, Rotation2d.fromDegrees(0)).asProxy()
);
addRequirements(turret);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,11 @@ public interface TurretIO {
@AutoLog
class TurretInputs{
// fields:
public double motorCurrentPositionDegrees = 0;
public double SetPointDegrees = 0;
public double turretCurrentPositionDegrees = 0;
public double turretSetPointDegrees = 0;
public double motorVoltage = 0;
public double motorCurrentAmps = 0;
public double motorVelocityRPM = 0;
public double motorVelocityRPS = 0;
public boolean forwardLimitSwitch = false;
public boolean reverseLimitSwitch = false;
}
Expand All @@ -21,6 +21,10 @@ class TurretInputs{

void setTurretPosition(Rotation2d position);

void resetEncoder(Rotation2d position);

void configurePID(double kP, double kI, double kD);

void updateInputs(TurretInputs inputs);
}

Loading

0 comments on commit 2eaa1ef

Please sign in to comment.