import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.commands.DoNothing;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
+import frc.robot.commands.gpm.AimAtPose;
import frc.robot.commands.vision.ShutdownAllPis;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
case SwerveCompetition:
case BetaBot:
- vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
+
// fall-through
case Vivace:
case Phil:
case Vertigo:
+ vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
turret = new Turret();
shooter = new Shooter();
LiveWindow.setEnabled(false);
SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis());
+ SmartDashboard.putData("Aim at thingy", new AimAtPose(drive, turret, new Pose2d(FieldConstants.field.getTagPose(26).get().getTranslation().toTranslation2d(), Rotation2d.kZero)));
}
/**
--- /dev/null
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.turret.Turret;
+
+/**
+ * Aims the robot at the closest April tag
+ */
+public class AimAtPose extends Command {
+ private Turret turret;
+ private Drivetrain drive;
+ private Pose2d target;
+
+ /**
+ * Aims the robot at the closest April tag
+ * @param drive The drivetrain
+ */
+ public AimAtPose(Drivetrain drive, Turret turret, Pose2d target){
+ this.turret = turret;
+ this.drive = drive;
+ this.target = target;
+ addRequirements(turret);
+ }
+
+ /**
+ * Gets the closest tag and sets the setpoint to aim at it
+ */
+ @Override
+ public void initialize(){}
+
+ @Override
+ public void execute() {
+ //Logger.recordOutput("TurretPose", new Pose2d(drive.getPose().getTranslation(), turret.getPosition()));
+ turret.setSetpoint(-target.getTranslation().minus(drive.getPose().getTranslation()).getAngle().getDegrees(), 0);
+ }
+
+ /**
+ * Stops the drivetrain
+ * @param interrupted If the command is interrupted
+ */
+ @Override
+ public void end(boolean interrupted) {
+
+ }
+
+ /**
+ * Returns if the command is finished
+ * @return If the PID is at the setpoint
+ */
+ @Override
+ public boolean isFinished() {
+ // return turret.atSetPoint();
+ return false;
+ }
+}
+
updateYawToTag();
if(turretVision != null && turretVisionEnabled && turret.atSetPoint()){
adjustWithTurretCam();
- turret.setSetpoint(adjustedSetpoint, drivetrain.getAngularRate(2));
+ turret.setSetpoint(adjustedSetpoint, -drivetrain.getAngularRate(2));
} else{
- turret.setSetpoint(turretSetpoint, drivetrain.getAngularRate(2));
+ turret.setSetpoint(turretSetpoint, -drivetrain.getAngularRate(2));
}
}
/** Location of hub target */
public static final Translation3d HUB_BLUE =
- new Translation3d(Units.inchesToMeters(156.8), 4.035, Units.inchesToMeters(72));
+ new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72));
public static Translation3d getHubTranslation() {
if (Robot.getAlliance() == Alliance.Blue) {
/**
* The maximum distance to the tag to use
*/
- public static final double MAX_DISTANCE = 2;
+ public static final double MAX_DISTANCE = 4;
/** If vision should use manual calculations */
public static final boolean USE_MANUAL_CALCULATIONS = true;
* <p>
* Only affects calculations using PhotonVision, not manual calculations.
*/
- public static final double HIGHEST_AMBIGUITY = 0.01;
+ public static final double HIGHEST_AMBIGUITY = 0.1;
public static final int MAX_EMPTY_TICKS = 10;
new Pair<String, Transform3d>(
"CameraRight",
new Transform3d(
- new Translation3d(Units.inchesToMeters(0.75), Units.inchesToMeters(11.5),
- Units.inchesToMeters(9.75)),
- new Rotation3d(0, Units.degreesToRadians(27), Units.degreesToRadians(10)))),
+ new Translation3d(Units.inchesToMeters(-11.5), Units.inchesToMeters(-1),
+ Units.inchesToMeters(10.5)),
+ new Rotation3d(0, Units.degreesToRadians(-29), Units.degreesToRadians(190)))),
new Pair<String, Transform3d>(
"CameraLeft",
new Transform3d(
- new Translation3d(Units.inchesToMeters(-13.5), Units.inchesToMeters(10.375),
- Units.inchesToMeters(11.625)),
- new Rotation3d(0, Units.degreesToRadians(27), 0)))));
+ new Translation3d(Units.inchesToMeters(-9.5), Units.inchesToMeters(13.5),
+ Units.inchesToMeters(13)),
+ new Rotation3d(0, Units.degreesToRadians(-29), Math.PI)))));
/**
* The transformations from the robot to object detection cameras
new Rotation3d(0, Units.degreesToRadians(20), 0))));
// used to cleanly shutdown the OrangePi
- public static final String[] ORANGEPI_HOSTNAMES = {"photonfront.local", "photonback.local"};
+ public static final String[] ORANGEPI_HOSTNAMES = {"photonvision.local"};
public static final String ORANGEPI_USERNAME = "pi";
public static final String ORANGEPI_PASSWORD = "raspberry";
}
interrupted->getDrivetrain().setStateDeadband(true),
()->false, getDrivetrain()).withTimeout(2));
- driver.get(PS5Button.LB).onTrue(
- new SequentialCommandGroup(
- new InstantCommand(()-> shooter.setShooter(-ShooterConstants.SHOOTER_VELOCITY)),
- new WaitCommand(0.8),
- new InstantCommand(()-> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER))
- )
- ).onFalse(
- new InstantCommand(() -> {
- shooter.setFeeder(0);
- shooter.setShooter(0);
- }));
+ // driver.get(PS5Button.LB).onTrue(
+ // new SequentialCommandGroup(
+ // new InstantCommand(()-> shooter.setShooter(-ShooterConstants.SHOOTER_VELOCITY)),
+ // new WaitCommand(0.8),
+ // new InstantCommand(()-> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER))
+ // )
+ // ).onFalse(
+ // new InstantCommand(() -> {
+ // shooter.setFeeder(0);
+ // shooter.setShooter(0);
+ // }));
//driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0)));
driver.get(PS5Button.SQUARE).onTrue(
})
);
+ driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse(
+ new InstantCommand(()->{
+ shooter.setFeeder(0);
+ })
+ );
+
// driver.get(PS5Button.CROSS).onTrue(
// new InstantCommand(()->{
// if(turretJoyStickAim == null || !turretJoyStickAim.isScheduled()){
public void periodic(){
shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
+ shooterMotorLeft.set(-1);
+ shooterMotorRight.set(-1);
feederMotor.set(feederPower);
}
package frc.robot.subsystems.shooter;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
public class ShooterConstants {
//TODO: find these values
public static final double FEEDER_RUN_POWER = 0.3; // meters per second??
- public static final double SHOOTER_VELOCITY = 10.0; // meters per second
+ public static double SHOOTER_VELOCITY = 30.0; // meters per second
public static final double SHOOTER_GEAR_RATIO = 36.0 / 24.0; // gear ratio from motors to shooter wheel
// public static final double SHOOTER_LAUNCH_DIAMETER = 0.0762; // meters (3 inches)
public static final double SHOOTER_LAUNCH_DIAMETER = 0.1016; // meters (4 inches) I think this is right.
// in m/s
public static final double EXIT_VELOCITY_TOLERANCE = 1.0;
+
+
}
// 8 velcocity is too little
// 16 is too much
\ No newline at end of file
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
+import frc.robot.subsystems.shooter.ShooterConstants;
public class Turret extends SubsystemBase implements TurretIO{
final private TalonFX motor;
private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
+ private double dV = 2.0;
+ private double kP = 10.0;
+ private double kI = 0.0;
+ private double kD = 0.0;
+
public Turret() {
motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN); // switch of course
config.Slot0.kV = 0.0; // Velocity gain: 1 rps -> 0.12V
config.Slot0.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters)
- config.Slot0.kP = 10.0; // If position error is 1 rotation, apply 10V
- config.Slot0.kI = 0.0; // Integral term (usually left at 0 for MotionMagic)
- config.Slot0.kD = 0.0; // Derivative term (used to dampen oscillations)
+ config.Slot0.kP = kP; // If position error is 1 rotation, apply 10V
+ config.Slot0.kI = kI; // Integral term (usually left at 0 for MotionMagic)
+ config.Slot0.kD = kD; // Derivative term (used to dampen oscillations)
MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY / TurretConstants.TURRET_RADIUS) * gearRatio; // max velocity * gear ratio
motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION / TurretConstants.TURRET_RADIUS) * gearRatio; // max Acceleration * gear ratio
- config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
motor.getConfigurator().apply(config);
// config.ClosedLoopGeneral.ContinuousWrap = true;
SmartDashboard.putData("Set to -180 degrees", new InstantCommand(() -> setSetpoint(-180, 0)));
SmartDashboard.putData("Set to -90 degrees", new InstantCommand(() -> setSetpoint(-90, 0)));
SmartDashboard.putData("Reset Yaw", new InstantCommand(() -> resetYaw()));
+ SmartDashboard.putNumber("Shooter Velocity", ShooterConstants.SHOOTER_VELOCITY);
}
public double getPosition() {
double motorTargetRotations = (setpoint / 360.0) * gearRatio;
//Tune this with rotating robot
- double dV = TurretConstants.ROTATIONAL_VELOCITY_CONSTANT;
motor.setControl(voltageRequest.withPosition(motorTargetRotations).withFeedForward(dV * robotRotVel));
}
}
updateInputs();
ligament2d.setAngle(getPosition());
- SmartDashboard.putNumber("Turret Position Degrees", getPosition());
+ dV = SmartDashboard.getNumber("FF Constant", dV);
+ kP = SmartDashboard.getNumber("kP Value", kP);
+ kI = SmartDashboard.getNumber("kI Value", kI);
+ kD = SmartDashboard.getNumber("kD Value", kD);
+
+ SmartDashboard.putNumber("Turret Position Degrees", getPosition());
+ SmartDashboard.putNumber("FF Constant", dV);
+ SmartDashboard.putNumber("kP Value", kP);
+ SmartDashboard.putNumber("kI Value", kI);
+ SmartDashboard.putNumber("kD Value", kD);
+
+
+ ShooterConstants.SHOOTER_VELOCITY = SmartDashboard.getNumber("Shooter Velocity", ShooterConstants.SHOOTER_VELOCITY);
+
}
@Override
public static double MAX_ANGLE = 180;
public static double MIN_ANGLE = -180;
- public static double MAX_VELOCITY = 2.0; // m/s
- public static double MAX_ACCELERATION = 10; // m/s^2
+ public static double MAX_VELOCITY = 10000000; // m/s
+ public static double MAX_ACCELERATION = 10000000; // m/s^2
public static double TURRET_WIDTH = Units.feetToMeters(1.0);
public static double TURRET_RADIUS = TURRET_WIDTH / 2;
- public static double ROTATIONAL_VELOCITY_CONSTANT = 0;
+ public static double ROTATIONAL_VELOCITY_CONSTANT = 0.2;
}
}
}
}
+
+ Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()];
+ for (int i = 0; i < FieldConstants.field.getTags().size(); i++) {
+ tags[i] = (FieldConstants.field.getTagPose(i+1).get());
+ }
+ Logger.recordOutput("AprilTags", tags);
}