Skip to content

Commit

Permalink
Merge branch 'zhangal/phoenixpro' into zhangal/poseEstimation
Browse files Browse the repository at this point in the history
  • Loading branch information
@mimizh2418
mimizh2418 committed Jan 28, 2024
2 parents ff8b891 + 274729a commit 2c65005
Show file tree
Hide file tree
Showing 16 changed files with 894 additions and 50 deletions.
64 changes: 59 additions & 5 deletions src/main/java/org/team1540/robot2024/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
package org.team1540.robot2024;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;

/**
Expand All @@ -16,9 +13,10 @@
*/
public final class Constants {
public static final boolean IS_COMPETITION_ROBOT = true;
// Whether to pull PID constants from SmartDashboard
public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY


public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;

public enum Mode {
Expand Down Expand Up @@ -46,6 +44,9 @@ public static class SwerveConfig {
public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 0;

// TODO: set this id
public static final int PIGEON_ID = 0;
}

public static class Drivetrain {
Expand All @@ -60,4 +61,57 @@ public static class Drivetrain {
public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
}

public static class Shooter {
public static class Flywheels {
// TODO: determine ids
public static final int LEFT_ID = 0;
public static final int RIGHT_ID = 0;

public static final double GEAR_RATIO = 24.0 / 36.0;
public static final double SIM_MOI = 4.08232288e-4;

// TODO: if it's tuned in simulation, it's tuned in real life
public static final double KP = 0.4;
public static final double KI = 0.0;
public static final double KD = 0.0;
public static final double KS = 0.01146;
public static final double KV = 0.07485; // TODO: this is what recalc says, may have to tune

public static final double ERROR_TOLERANCE_RPM = 50;
}

public static class Pivot {
// TODO: determine ids
public static final int MOTOR_ID = 0;
public static final int CANCODER_ID = 0;

// TODO: figure this out
public static final double CANCODER_OFFSET_ROTS = 0;
// TODO: determine ratios
public static final double CANCODER_TO_PIVOT = 60.0 / 20.0;
public static final double MOTOR_TO_CANCODER = 33.0;
public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT;
public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910);
// TODO: find the moi
public static final double SIM_MOI = 0.22552744227754662;

public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(60.0);
public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(8.0);

// TODO: tune pid
public static final double KP = 0.1;
public static final double KI = 0.0;
public static final double KD = 0.0;
public static final double KS = 0.0;
public static final double KG = 0.1;
public static final double KV = 0.1;

public static final double CRUISE_VELOCITY_RPS = 4.0;
public static final double MAX_ACCEL_RPS2 = 40.0;
public static final double JERK_RPS3 = 2000;

public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2);
}
}
}
35 changes: 23 additions & 12 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,11 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;


import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.commands.FeedForwardCharacterization;
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.util.swerve.SwerveFactory;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

Expand All @@ -27,6 +28,7 @@
public class RobotContainer {
// Subsystems
public final Drivetrain drivetrain;
public final Shooter shooter;

// Controller
public final CommandXboxController driver = new CommandXboxController(0);
Expand All @@ -35,6 +37,10 @@ public class RobotContainer {
// Dashboard inputs
public final LoggedDashboardChooser<Command> autoChooser;

// TODO: testing dashboard inputs, remove for comp
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
Expand All @@ -44,11 +50,12 @@ public RobotContainer() {
// Real robot, instantiate hardware IO implementations
drivetrain =
new Drivetrain(
new GyroIONavx(),
new GyroIOPigeon2(),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
break;

case SIM:
Expand All @@ -60,22 +67,19 @@ public RobotContainer() {
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim());
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
break;

default:
// Replayed robot, disable IO implementations
drivetrain =
new Drivetrain(
new GyroIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
});
new GyroIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {});
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
break;
}

Expand All @@ -88,6 +92,10 @@ public RobotContainer() {
"Drive FF Characterization",
new FeedForwardCharacterization(
drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
autoChooser.addOption(
"Flywheels FF Characterization",
new FeedForwardCharacterization(
shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));

// Configure the button bindings
configureButtonBindings();
Expand All @@ -108,6 +116,9 @@ private void configureButtonBindings() {
drivetrain
).ignoringDisable(true)
);

copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
}

/**
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import org.team1540.robot2024.util.PolynomialRegression;
import org.team1540.robot2024.util.math.PolynomialRegression;

import java.util.LinkedList;
import java.util.List;
Expand Down
11 changes: 8 additions & 3 deletions src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java
View file
Original file line number Diff line number Diff line change
@@ -1,18 +1,20 @@
package org.team1540.robot2024.subsystems.drive;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;

import static org.team1540.robot2024.Constants.SwerveConfig.*;

/**
* IO implementation for Pigeon2
*/
public class GyroIOPigeon2 implements GyroIO {
private final Pigeon2 pigeon = new Pigeon2(20);
private final Pigeon2 pigeon = new Pigeon2(PIGEON_ID);
private final StatusSignal<Double> yaw = pigeon.getYaw();
private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZDevice();

Expand All @@ -22,11 +24,14 @@ public GyroIOPigeon2() {
yaw.setUpdateFrequency(100.0);
yawVelocity.setUpdateFrequency(100.0);
pigeon.optimizeBusUtilization();

PhoenixTimeSyncSignalRefresher.registerSignals(yaw, yawVelocity);
}

@Override
public void updateInputs(GyroIOInputs inputs) {
inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
PhoenixTimeSyncSignalRefresher.refreshSignals();
inputs.connected = BaseStatusSignal.isAllGood(yaw, yawVelocity);
inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
}
Expand Down
40 changes: 12 additions & 28 deletions src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.team1540.robot2024.subsystems.drive;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.controls.VoltageOut;
Expand All @@ -11,10 +10,10 @@
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
import org.team1540.robot2024.util.swerve.SwerveFactory;

import static org.team1540.robot2024.Constants.Drivetrain.*;
import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;

/**
* Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
Expand Down Expand Up @@ -44,15 +43,11 @@ public class ModuleIOTalonFX implements ModuleIO {
private final StatusSignal<Double> turnAppliedVolts;
private final StatusSignal<Double> turnCurrent;

private final boolean isCANFD;

public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
driveTalon = hw.driveMotor;
turnTalon = hw.turnMotor;
cancoder = hw.cancoder;

isCANFD = CANBus.isNetworkFD(CAN_BUS);

setDriveBrakeMode(true);
setTurnBrakeMode(true);

Expand All @@ -79,32 +74,21 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
turnCurrent);
driveTalon.optimizeBusUtilization();
turnTalon.optimizeBusUtilization();

PhoenixTimeSyncSignalRefresher.registerSignals(drivePosition, turnPosition);
}

@Override
public void updateInputs(ModuleIOInputs inputs) {
if (isCANFD) {
BaseStatusSignal.waitForAll(0.01, drivePosition, turnPosition);
BaseStatusSignal.refreshAll(
driveVelocity,
driveAppliedVolts,
driveCurrent,
turnAbsolutePosition,
turnVelocity,
turnAppliedVolts,
turnCurrent);
} else {
BaseStatusSignal.refreshAll(
drivePosition,
driveVelocity,
driveAppliedVolts,
driveCurrent,
turnAbsolutePosition,
turnPosition,
turnVelocity,
turnAppliedVolts,
turnCurrent);
}
PhoenixTimeSyncSignalRefresher.refreshSignals();
BaseStatusSignal.refreshAll(
driveVelocity,
driveAppliedVolts,
driveCurrent,
turnAbsolutePosition,
turnVelocity,
turnAppliedVolts,
turnCurrent);

inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO;
inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO;
Expand Down
37 changes: 37 additions & 0 deletions src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
package org.team1540.robot2024.subsystems.shooter;

import org.littletonrobotics.junction.AutoLog;

public interface FlywheelsIO {
@AutoLog
class FlywheelsIOInputs {
public double leftAppliedVolts = 0.0;
public double leftCurrentAmps = 0.0;
public double leftVelocityRPM = 0.0;

public double rightAppliedVolts = 0.0;
public double rightCurrentAmps = 0.0;
public double rightVelocityRPM = 0.0;
}

/**
* Updates the set of loggable inputs
*/
default void updateInputs(FlywheelsIOInputs inputs) {}

/**
* Runs open loop at the specified voltages
*/
default void setVoltage(double leftVolts, double rightVolts) {}


/**
* Runs closed loop at the specified RPMs
*/
default void setSpeeds(double leftRPM, double rightRPM) {}

/**
* Configures the PID controller
*/
default void configPID(double kP, double kI, double kD) {}
}
Loading

0 comments on commit 2c65005

Please sign in to comment.