target == FieldConstants.getHubTranslation().toTranslation2d() ?
FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub
- goalState = ShooterPhysics.getShotParams(
+ var goalStateWithT = ShooterPhysics.getShotParamsWithT(
+ Translation2d.kZero,
target3d.minus(lookahead3d),
target == FieldConstants.getHubTranslation().toTranslation2d() ?
- 2.0 : 3.0);
- goalState = goalStateWithT.getFirst();
+ 2.0 : 5.0);
++
++ goalState = goalStateWithT.getFirst();
- timeOfFlight = goalStateWithT.getSecond();
+ double TOFAdjustment = 0.75;
- timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
++ timeOfFlight = goalStateWithT.getSecond() * TOFAdjustment;
double offsetX = turretVelocityX * timeOfFlight;
double offsetY = turretVelocityY * timeOfFlight;
lookaheadPose =
import frc.robot.commands.gpm.ClimbDriveCommand;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.ReverseMotors;
+import frc.robot.commands.gpm.Superstructure;
import frc.robot.constants.Constants;
import frc.robot.subsystems.Climb.LinearClimb;
+ import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.drivetrain.Drivetrain;
+ import frc.robot.subsystems.hood.Hood;
+ import frc.robot.subsystems.hood.HoodConstants;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.subsystems.turret.Turret;
double motorRotations = turretRot * TurretConstants.GEAR_RATIO;
//Sets the initial motor position
+ inputs.positionDeg = turretRot;
motor.setPosition(motorRotations);
+ //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
+
+ motor.setPosition(0.0);
+
SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));
SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);}));