Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

oaifh

Browse Source
This commit is contained in:
aarav18
2022-03-24 17:51:18 -06:00
parent 7168e7b618
commit fcf46c9cd7
3 changed files with 28 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+1
View File
@@ -140,6 +140,7 @@ public class Robot extends TimedRobot {
@Override @Override
public void robotPeriodic() { public void robotPeriodic() {
m_robotTime.updateTimes(); m_robotTime.updateTimes();
System.out.println((180.0 / Math.PI) * Math.atan2(-(219.25 / 2.00) - 40.44 + 10.00, -(82.83 / 2.00) + 15.56));
// current = // current =
// m_robotContainer.m_robotBoomBoom.getCurrent() + // m_robotContainer.m_robotBoomBoom.getCurrent() +
// m_robotContainer.m_robotClimber.getCurrent(); //+ // m_robotContainer.m_robotClimber.getCurrent(); //+
src/main/java/frc4388/robot/RobotContainer.java
+13 -14
View File
@@ -129,8 +129,6 @@ public class RobotContainer {
*/ */
public RobotContainer() { public RobotContainer() {
// double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate. // double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
// double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 // double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
@@ -475,22 +473,23 @@ public class RobotContainer {
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs. Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
// SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new ParallelCommandGroup( new ParallelCommandGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0) new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot // weirdAutoShootingGroup, // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
// ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY) // ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY)
return new SequentialCommandGroup( //new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
// weirdAutoShootingGroup, // * shoot new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.5, 0.0, 0.0}, 1.0), // * drive out of tarmac
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(-(219.25 / 2.00) - turretDistanceFromFront, -(82.83 / 2.00) + 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving weirdAutoShootingGroup, // * shoot
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.6, 0.0, 0.0}, 0.5))); // * drive off line new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
);
// ! TWO BALL AUTO (HOPEFULLY) // ! TWO BALL AUTO (HOPEFULLY)
// return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+14 -14
View File
@@ -35,7 +35,7 @@ public class TrackTarget extends CommandBase {
BoomBoom m_boomBoom; BoomBoom m_boomBoom;
Hood m_hood; Hood m_hood;
// boolean isAuto; boolean isAuto;
static double velocity; static double velocity;
static double hoodPosition; static double hoodPosition;
@@ -55,22 +55,22 @@ public class TrackTarget extends CommandBase {
long startTime; long startTime;
private double timeTolerance; private double timeTolerance;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry/*, boolean isAuto*/) { public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
m_turret = turret; m_turret = turret;
m_boomBoom = boomBoom; m_boomBoom = boomBoom;
m_hood = hood; m_hood = hood;
m_visionOdometry = visionOdometry; m_visionOdometry = visionOdometry;
// this.isAuto = isAuto; this.isAuto = isAuto;
this.timeTolerance = 1000; this.timeTolerance = 1000;
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
SmartDashboard.putNumber("Distance Adjust", -35); SmartDashboard.putNumber("Distance Adjust", -35);
} }
// public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
// this(turret, boomBoom, hood, visionOdometry, false); this(turret, boomBoom, hood, visionOdometry, false);
// } }
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
@@ -195,14 +195,14 @@ public class TrackTarget extends CommandBase {
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
// if (this.isAuto) { if (this.isAuto) {
// if (targetLocked && !timerStarted) { if (targetLocked && !timerStarted) {
// timerStarted = true; timerStarted = true;
// startTime = System.currentTimeMillis(); startTime = System.currentTimeMillis();
// } }
// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
// } else { } else {
return false; return false;
// } }
} }
} }