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

Commit

Permalink
feat: shooter tuning and indexer tuning
Browse files Browse the repository at this point in the history
  • Loading branch information
mimizh2418 committed Dec 15, 2023
1 parent 03beb15 commit 6c2151f
Show file tree
Hide file tree
Showing 6 changed files with 57 additions and 9 deletions.
8 changes: 4 additions & 4 deletions src/main/java/org/team1540/bunnybotTank2023/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -63,11 +63,11 @@ public static class ShooterConstants {
public static final int LEADER_ID = 20;
public static final int FOLLOWER_ID = 21;

public static final double KP = 0.6; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm
public static final double KI = 0.07;
public static final double KP = 0.4; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm
public static final double KI = 0.05;
public static final double KD = 0;
public static final double KS = 0.19;
public static final double KV = 0.1149;
public static final double KS = 0;
public static final double KV = 0.113;

public static final double ERROR_TOLERANCE_RPM = 30;
public static final double SHOOTER_IDLE_RPM = 1000;
Expand Down
24 changes: 22 additions & 2 deletions src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,20 @@
package org.team1540.bunnybotTank2023;

import edu.wpi.first.wpilibj2.command.Command;
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.LoggedDashboardInput;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes;
import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain;
import org.team1540.bunnybotTank2023.commands.drivetrain.TankdriveCommand;
import org.team1540.bunnybotTank2023.commands.indexer.Indexer;
import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand;
import org.team1540.bunnybotTank2023.commands.shooter.Shooter;
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.shooter.ShooterIOReal;
import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim;

Expand All @@ -29,20 +35,25 @@ public class RobotContainer {
// Subsystems
Drivetrain drivetrain;
Shooter shooter;
Indexer indexer;

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

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

public RobotContainer() {
if (Robot.isReal()) {
// Initialize subsystems with hardware IO
drivetrain = new Drivetrain(new DrivetrainIOReal());
shooter = new Shooter(new ShooterIOReal());
indexer = new Indexer(new IndexerIOReal());
} else {
// Initialize subsystems with simulation IO
drivetrain = new Drivetrain(new DrivetrainIOSim());
shooter = new Shooter(new ShooterIOSim());
indexer = null;
}
setDefaultCommands();
configureButtonBindings();
Expand All @@ -51,13 +62,22 @@ public RobotContainer() {

/** Use this method to define your trigger->command mappings. */
private void configureButtonBindings() {
driver.a()
.whileTrue(new InstantCommand(() -> shooter.setVelocity(shooterSpeed.get()))
.andThen(new IndexerCommand(indexer)))
.onFalse(new InstantCommand(() -> {
shooter.stop();
indexer.stop();
}));

driver.b().whileTrue(new IndexerCommand(indexer));
}

private void setDefaultCommands() {
drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver));
// drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver));
shooter.setDefaultCommand(new StartEndCommand(
() -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
() -> shooter.stop(),
() -> {},
shooter
));
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,9 @@ public void setTopSpeed(double speed) {
public void setBottomSpeed(double speed) {
io.setBottomPercent(speed);
}

public void stop() {
setTopSpeed(0);
setBottomSpeed(0);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
package org.team1540.bunnybotTank2023.commands.indexer;

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

public class IndexerCommand extends CommandBase {
private final Indexer indexer;

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

@Override
public void initialize() {
indexer.setBottomSpeed(0.75);
indexer.setTopSpeed(0.75);
}

@Override
public void end(boolean interrupted) {
indexer.stop();
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ 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 @@ -15,12 +15,12 @@ public IndexerIOReal() {

topMotor1.setSmartCurrentLimit(30);
topMotor2.setSmartCurrentLimit(30);
bottomMotor.setSmartCurrentLimit(30);
bottomMotor.setSmartCurrentLimit(25);

topMotor2.follow(topMotor1);
topMotor2.follow(topMotor1, true);

topMotor1.setInverted(false);
topMotor2.setInverted(true);
// topMotor2.setInverted(false);
bottomMotor.setInverted(true);
}

Expand Down

0 comments on commit 6c2151f

Please sign in to comment.