package frc.robot.commands.gpm;
-import java.util.Optional;
-
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.MathUtil;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.hood.HoodConstants;
import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.ShooterConstants;
import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.subsystems.turret.ShotInterpolation;
import frc.robot.subsystems.turret.Turret;
private TurretState goalState;
- private final double phaseDelay = 0.03;
+ private final double phaseDelay = 0.03; // Extrapolation delay due to latency
private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
- private double velocityAdjustment = 0;
-
public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
this.turret = turret;
this.drivetrain = drivetrain;
public void updateSetpoints(Pose2d drivepose) {
Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
- double turretToTargetDistance = target.getDistance(turretPosition.getTranslation());
// If the robot is moving, adjust the target position based on velocity
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+ // Rotational adjustment is not being used, since turret is in center of robot
double turretVelocityX =
fieldRelVel.vxMetersPerSecond
+ fieldRelVel.omegaRadiansPerSecond
double timeOfFlight;
Pose2d lookaheadPose = turretPosition;
- //double lookaheadTurretToTargetDistance = turretToTargetDistance;
/*
* Loop (20) until lookaheadPose converges BECAUSE -->
new Pose2d(
turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
turretPosition.getRotation());
- //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation());
}
+ // Get the field angle relative to the target pose
turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
if (lastTurretAngle == null) {
lastTurretAngle = turretAngle;
}
+
+ // Take the filtered average as the turret's velocity when robot is moving translationally
turretVelocity =
- turretAngleFilter.calculate(
- turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+ turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+
lastTurretAngle = turretAngle;
Logger.recordOutput("Lookahead Pose", lookaheadPose);
+ // Subtract the rotational angle of the robot from the setpoint
double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
// Shortest path
double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error;
+
// Stay within +/- 200 -- if shortest path is past 200, we go long way around
double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE;
if (potentialSetpoint > turretRange/2) {
hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
lastHoodAngle = hoodAngle;
- if (Math.abs(lastTurretAngle.getDegrees() - turretSetpoint) > 90) {
- velocityAdjustment = -drivetrain.getAngularRate(2) * 1.0;
- }
}
public void updateDrivePose(){
Pose2d currentPose = drivetrain.getPose();
- // Add 180 degrees to the rotation bc robot is backwards
+
drivepose = new Pose2d(
currentPose.getTranslation(),
+ // Uncomment this if robot is backwards
currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
);
ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+
+ // Add a phase delay extrapolation component for latency delay
drivepose.exp(
new Twist2d(
robotRelVel.vxMetersPerSecond * phaseDelay,
updateSetpoints(drivepose);
turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
- //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
- //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
+ hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
+ shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
@Override
public void end(boolean interrupted) {
- // Set the turret to a safe position when the command ends
+ // Set the turret and hood to a safe position when the command ends
turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
shooter.setShooter(0.0);
- //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
+ hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
if(spindexer != null){
spindexer.stopSpindexer();
}
import org.littletonrobotics.junction.Logger;
-import com.ctre.phoenix6.CANBus;
-import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.filter.LinearFilter;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
-import yams.gearing.GearBox;
public class Hood extends SubsystemBase implements HoodIO{
private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import au.grapplerobotics.ConfigurationFailedException;
-import au.grapplerobotics.LaserCan;
-import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
-import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
-import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
-import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
// Goal Velocity / Double theCircumfrence
private double shooterTargetSpeed = 0;
- private double feederPower = 0;
-
- public boolean shooterAtMaxSpeed = false;
- public boolean ballDetected = false;
- //Velocity in rotations per second
+ // Velocity in rotations per second
VelocityVoltage voltageRequest = new VelocityVoltage(0);
private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
- double powerModifier;
+ double powerModifier = 1.0;
public Shooter(){
-
updateInputs();
TalonFXConfiguration config = new TalonFXConfiguration();
powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier);
SmartDashboard.putNumber("shooter power modifier", powerModifier);
- shooterMotorLeft.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2))));
- shooterMotorRight.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2))));
+
+ // Convert to RPS
+ double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2));
+
+ // Sets the motor control to target velocity
+ shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS));
+ shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS));
Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
}
+ /**
+ * Sets the target speed of the shooter
+ * @param linearVelocityMps
+ */
public void setShooter(double linearVelocityMps) {
shooterTargetSpeed = linearVelocityMps;
}
Logger.processInputs("Shooter", inputs);
}
+ /**
+ * @return Whether the shooter is at the target speed with tolerance of 1 m/s
+ */
public boolean atTargetSpeed(){
return Math.abs(getShooterVelcoity() - shooterTargetSpeed) < 1.0;
}
+ /**
+ * @return Gets the target velocity in m/s
+ */
@AutoLogOutput(key="Shooter/TargetSpeed")
public double getTargetVelocity(){
return Units.rotationsToRadians(shooterTargetSpeed);
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IdConstants;
-import frc.robot.subsystems.spindexer.SpindexerIO;
public class Spindexer extends SubsystemBase implements SpindexerIO{
TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID);
public void periodic() {
power = SmartDashboard.getNumber("Spindexer Power", power);
SmartDashboard.putNumber("Spindexer Power", power);
+
motor.set(power);
if (inputs.spindexerVelocity < SpindexerConstants.spindexerVelocityWithBall) {
ballCount++;
}
}
+ /**
+ * @return
+ */
public void maxSpindexer(){
- power = 1.0;
+ power = 0.5;
}
public void stopSpindexer(){
private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE);
private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE);
- private static final double MAX_VEL_RAD_PER_SEC = 600;
- private static final double MAX_ACCEL_RAD_PER_SEC2 = 120.0;
+ private static final double MAX_VEL_RAD_PER_SEC = TurretConstants.MAX_VELOCITY;
+ private static final double MAX_ACCEL_RAD_PER_SEC2 = TurretConstants.MAX_ACCELERATION;
private static final double EXTRAPOLATION_TIME_CONSTANT = 0.06;
private double FEEDFORWARD_KV = 0.185;
- private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02
- , 0.02);
+ // Super low magnitude filter for the position to make it less jittery
+ private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02);
private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
private final MechanismRoot2d root = mech.getRoot("turret", 50, 50);
private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0));
- // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010);
private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
/* ---------------- Constructor ---------------- */
0.0);
}
+ //TODO: replace this stuff with Chinese Remainder Theorem calculator -- ignore this for now
+
double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET);
double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
motor.setPosition(motorRotations);
- motor.setPosition(0.0); //TODO: remove after hardcrt works
+ motor.setPosition(0.0); //TODO: remove after chinese remainder theorem works
SmartDashboard.putData("Turret Mech", mech);
/* ---------------- Public API ---------------- */
+ /**
+ * Sets the setpoint position and velocity of the turret. Call in command execute.
+ * @param angle
+ * @param velocityRadPerSec
+ */
public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {
goalAngle = angle;
goalVelocityRadPerSec = velocityRadPerSec;
return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(2.0);
}
+ /**
+ * @return Posiiton of the turret in radians
+ */
public double getPositionRad() {
return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
}
+ /**
+ * @return Posiiton of the turret in radians
+ */
+ public double getPositionDeg() {
+ return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
+ }
+
/* ---------------- Periodic ---------------- */
@Override
// Tells the Kraken to get to this position using 1000Hz profile
double motorGoalRotations = Units.radiansToRotations(best) * GEAR_RATIO;
+ // Clamp position setpoint to min and max angles
motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO);
+ // Multiply goal velocity by kV
double robotTurnCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV;
+ // Sets motor control with feedforward
motor.setControl(mmVoltageRequest
.withPosition(motorGoalRotations)
.withFeedForward(robotTurnCompensation));
inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
}
+ // Also ignore this for now
private double wrapUnit(double value) {
value %= 1.0;
if (value < 0) value += 1.0;
public static double MAX_ANGLE = 200;
public static double MIN_ANGLE = -200;
- public static double MAX_VELOCITY = 10000000; // m/s
- public static double MAX_ACCELERATION = 10000000; // m/s^2
+ public static double MAX_VELOCITY = 600; // rad/s
+ public static double MAX_ACCELERATION = 120.0; // rad/s^2
+ // Not using this, but just in case
public static double TURRET_WIDTH = Units.inchesToMeters(6.4);
public static double TURRET_RADIUS = TURRET_WIDTH / 2;
public static double LEFT_ENCODER_OFFSET = 9.52; // degrees 9.52 rot
public static double RIGHT_ENCODER_OFFSET = 6.53; // degrees 6.53 rot
+ // Turret is in center of robot, but make use of the height in shooter physics
public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters
public static double CRT_TOLERANCE = 0.01;
+++ /dev/null
-
-package frc.robot.util;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import frc.robot.util.ChineseRemainderTheorum.Encoder;
-
-public class ChineseRemainderTheorumTest {
-
- @BeforeEach
- public void prepare() {
- }
-
- @AfterEach
- public void cleanup() {
- }
-
- @Test
- public void test() {
- double tolerance = 0.01;
-
- Encoder a = new Encoder(5000 % 123, 123);
- Encoder b = new Encoder(5000 % 321, 321);
- double val = ChineseRemainderTheorum.compute(a, b, tolerance);
- assertEquals(5000, val, tolerance);
- }
-}