From 6812e4150e5ed0c1cb13bce75051a8110a899fb0 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 30 Oct 2022 17:54:23 +0200 Subject: [PATCH 01/89] Amir - added getLocation command --- src/main/java/edu/greenblitz/pegasus/OI.java | 2 ++ .../java/edu/greenblitz/pegasus/Robot.java | 8 +++++- .../pegasus/commands/shooter/getLocation.java | 8 ++++++ .../pegasus/subsystems/Limelight.java | 28 +++++++++++++++++++ 4 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java create mode 100644 src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 154b8c1..6bf64cf 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -15,6 +15,7 @@ import edu.greenblitz.pegasus.commands.shooter.FlipShooter; import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; +import edu.greenblitz.pegasus.commands.shooter.getLocation; import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement; import edu.greenblitz.pegasus.commands.swerve.SwerveCommand; import edu.greenblitz.pegasus.commands.swerve.garbage.CalibrateMaxMin; @@ -103,6 +104,7 @@ public boolean isFinished() { } private void initRealButtons() { + mainJoystick.A.whileHeld(new getLocation()); } private void initAmirButtons() { diff --git a/src/main/java/edu/greenblitz/pegasus/Robot.java b/src/main/java/edu/greenblitz/pegasus/Robot.java index d4c7047..c45dbc9 100644 --- a/src/main/java/edu/greenblitz/pegasus/Robot.java +++ b/src/main/java/edu/greenblitz/pegasus/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.net.PortForwarder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -48,7 +49,12 @@ public void robotInit() { Indexing.getInstance(); Shooter.create(new SparkMaxFactory().withInverted(true).withRampRate(0.4).withCurrentLimit(30), RobotMap.Pegasus.Shooter.ShooterMotor.PORT_LEADER); - + PortForwarder.add(5800, "limelight.local", 5800); + PortForwarder.add(5801, "limelight.local", 5801); + PortForwarder.add(5802, "limelight.local", 5802); + PortForwarder.add(5803, "limelight.local", 5803); + PortForwarder.add(5804, "limelight.local", 5804); + PortForwarder.add(5805, "limelight.local", 5805); //todo add voltage compensation //swerve SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(RobotMap.Pegasus.Swerve.ks, RobotMap.Pegasus.Swerve.kv, RobotMap.Pegasus.Swerve.ka); diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java new file mode 100644 index 0000000..6780741 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java @@ -0,0 +1,8 @@ +package edu.greenblitz.pegasus.commands.shooter; + +public class getLocation extends ShooterCommand{ + @Override + public void initialize() { + super.initialize(); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java new file mode 100644 index 0000000..52ecf7c --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -0,0 +1,28 @@ +package edu.greenblitz.pegasus.subsystems; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.GBSubsystem; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Limelight extends GBSubsystem { + private static Limelight instance; + + public static void init() { + instance = new Limelight(); + } + + public static Limelight getInstance() { + if (instance == null) { + init(); + } + return instance; + } + + public double getLocation() { + double arr = -1; + NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("angle"); + double angle = loc.getDouble(1); + return angle; + } + +} From 45805ff016737be26c1254294416c3a53d615d45 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 30 Oct 2022 20:09:06 +0200 Subject: [PATCH 02/89] Amir - added getlocation function --- src/main/java/edu/greenblitz/pegasus/OI.java | 12 +++++----- .../commands/shooter/FindLocation.java | 24 +++++++++++++++++++ .../pegasus/commands/shooter/getLocation.java | 8 ------- .../pegasus/subsystems/Limelight.java | 17 ++++++++----- 4 files changed, 41 insertions(+), 20 deletions(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 6bf64cf..4e7a1f9 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -12,10 +12,10 @@ import edu.greenblitz.pegasus.commands.intake.roller.RunRoller; import edu.greenblitz.pegasus.commands.multiSystem.EjectEnemyBallFromGripper; import edu.greenblitz.pegasus.commands.multiSystem.InsertIntoShooter; +import edu.greenblitz.pegasus.commands.shooter.FindLocation; import edu.greenblitz.pegasus.commands.shooter.FlipShooter; import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; -import edu.greenblitz.pegasus.commands.shooter.getLocation; import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement; import edu.greenblitz.pegasus.commands.swerve.SwerveCommand; import edu.greenblitz.pegasus.commands.swerve.garbage.CalibrateMaxMin; @@ -27,7 +27,7 @@ public class OI { public enum IOModes { - DEBUG,/* REAL,*/ AMIR + DEBUG, REAL, AMIR } private static final IOModes IOMode = IOModes.AMIR; //decides which set of controls to init. @@ -44,9 +44,9 @@ private OI() { case DEBUG: initDebugButtons(); break; -// case REAL: -// initRealButtons(); -// break; + case REAL: + initRealButtons(); + break; case AMIR: initAmirButtons(); //todo do i really need to explain @@ -104,7 +104,7 @@ public boolean isFinished() { } private void initRealButtons() { - mainJoystick.A.whileHeld(new getLocation()); + mainJoystick.A.whileHeld(new FindLocation()); } private void initAmirButtons() { diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java new file mode 100644 index 0000000..0f1329b --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java @@ -0,0 +1,24 @@ +package edu.greenblitz.pegasus.commands.shooter; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.pegasus.subsystems.Limelight; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class FindLocation extends GBCommand { + public FindLocation(){ + super(); + require((Subsystem) Limelight.getInstance()); + } + + @Override + public void initialize() { + System.out.println(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv")); + System.out.println((Limelight.getInstance().getLocation())); + } + + @Override + public boolean isFinished() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java deleted file mode 100644 index 6780741..0000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java +++ /dev/null @@ -1,8 +0,0 @@ -package edu.greenblitz.pegasus.commands.shooter; - -public class getLocation extends ShooterCommand{ - @Override - public void initialize() { - super.initialize(); - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java index 52ecf7c..af5823a 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -4,6 +4,9 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.Arrays; +import java.util.function.Supplier; + public class Limelight extends GBSubsystem { private static Limelight instance; @@ -18,11 +21,13 @@ public static Limelight getInstance() { return instance; } - public double getLocation() { - double arr = -1; - NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("angle"); - double angle = loc.getDouble(1); - return angle; + public double[] getLocation() { + double[] arr = {-1, -1}; + NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("llpython"); + if (loc != null) { + System.out.println(Arrays.toString(loc.getDoubleArray(arr))); + } + return new double[5]; } -} + } From 9376c5f2039b0b68d5333c2404d5824d32a5c7ba Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 30 Oct 2022 21:42:25 +0200 Subject: [PATCH 03/89] Romy & Amir - made combineJoystickMovment by supplier --- .../swerve/CombineJoystickMovement.java | 7 ++- .../commands/swerve/MoveBySupplier.java | 52 +++++++++++++++++++ .../pegasus/commands/swerve/MoveToTarget.java | 10 ++++ .../pegasus/subsystems/Limelight.java | 4 +- 4 files changed, 70 insertions(+), 3 deletions(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index f064a7e..18be094 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -3,6 +3,8 @@ import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; import edu.greenblitz.pegasus.RobotMap; +import java.util.function.DoubleSupplier; + public class CombineJoystickMovement extends SwerveCommand { static double ANG_SPEED_FACTOR = 5;//todo magic number @@ -16,8 +18,9 @@ public CombineJoystickMovement(SmartJoystick joystick, boolean isSlow) { this.joystick = joystick; this.isSlow = isSlow; if (isSlow) { - ANG_SPEED_FACTOR *= 0.5; //todo querry from robot map in initialize to prevent repeated changes - LIN_SPEED_FACTOR *= 0.3; + ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes + LIN_SPEED_FACTOR *= 0.5; + } } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java new file mode 100644 index 0000000..0397330 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java @@ -0,0 +1,52 @@ +package edu.greenblitz.pegasus.commands.swerve; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; +import edu.greenblitz.pegasus.RobotMap; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class MoveBySupplier extends SwerveCommand { + + //we mean left, people are dumb + static double ANG_SPEED_FACTOR = 5;//todo magic number + static double LIN_SPEED_FACTOR = RobotMap.Pegasus.Swerve.MAX_VELOCITY; + DoubleSupplier forward; + DoubleSupplier right; + DoubleSupplier ang; + + public final SmartJoystick joystick; + + private boolean isSlow; + + //supplier returns value 0-1 + public MoveBySupplier(SmartJoystick joystick, boolean isSlow, DoubleSupplier forward, DoubleSupplier right, DoubleSupplier ang) { + this.joystick = joystick; + this.isSlow = isSlow; + this.forward = forward; + this.right = right; + this.ang = ang; + } + + @Override + public void initialize() { + if (isSlow) { + ANG_SPEED_FACTOR *= 0.5; //todo querry from robot map in initialize to prevent repeated changes + LIN_SPEED_FACTOR *= 0.5; + } + } + + public void execute() { + double rightwardSpeed = -right.getAsDouble() * LIN_SPEED_FACTOR; + double forwardSpeed = forward.getAsDouble() * LIN_SPEED_FACTOR; + double angSpeed = ang.getAsDouble() * ANG_SPEED_FACTOR; + if (forwardSpeed == 0 && rightwardSpeed == 0 && angSpeed == 0) { + swerve.stop(); + return; + } + swerve.MoveByChassisSpeeds(forwardSpeed, rightwardSpeed, angSpeed, + -swerve.getChassisAngle() ); + }; + +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java new file mode 100644 index 0000000..3c78d33 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java @@ -0,0 +1,10 @@ +package edu.greenblitz.pegasus.commands.swerve; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; + +public class MoveToTarget extends GBCommand { + public MoveToTarget(double targetAngle){ + SwerveChassis.getInstance().moveChassisLin(); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java index af5823a..9f4e853 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -30,4 +30,6 @@ public double[] getLocation() { return new double[5]; } - } +} + + From fd257d45fc94fcc9f9d1f5c81b3ec71953a21111 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 6 Nov 2022 17:33:07 +0200 Subject: [PATCH 04/89] Romy & Amir - fixed conflicts, please rebase more often --- src/main/java/edu/greenblitz/pegasus/OI.java | 30 +++++++++++-- .../java/edu/greenblitz/pegasus/RobotMap.java | 2 +- .../commands/shooter/FindLocation.java | 4 ++ .../swerve/CombineJoystickMovement.java | 45 ++++++++++++++----- .../commands/swerve/MoveByVisionSupplier.java | 11 +++++ .../pegasus/commands/swerve/MoveToTarget.java | 10 ----- .../pegasus/subsystems/Limelight.java | 9 +++- 7 files changed, 82 insertions(+), 29 deletions(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveByVisionSupplier.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 4e7a1f9..ecd2dfd 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -1,11 +1,9 @@ package edu.greenblitz.pegasus; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.commands.DoUntilCommand; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; import edu.greenblitz.pegasus.commands.funnel.RunFunnel; -import edu.greenblitz.pegasus.commands.handleBalls.HandleBalls; import edu.greenblitz.pegasus.commands.intake.extender.ExtendRoller; import edu.greenblitz.pegasus.commands.intake.extender.RetractRoller; import edu.greenblitz.pegasus.commands.intake.extender.ToggleRoller; @@ -17,11 +15,15 @@ import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement; +import edu.greenblitz.pegasus.commands.swerve.MoveByVisionSupplier; import edu.greenblitz.pegasus.commands.swerve.SwerveCommand; import edu.greenblitz.pegasus.commands.swerve.garbage.CalibrateMaxMin; +<<<<<<< HEAD import edu.greenblitz.pegasus.subsystems.Indexing; import edu.greenblitz.pegasus.utils.DigitalInputMap; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +======= +>>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; public class OI { @@ -29,8 +31,14 @@ public class OI { public enum IOModes { DEBUG, REAL, AMIR } +<<<<<<< HEAD private static final IOModes IOMode = IOModes.AMIR; //decides which set of controls to init. +======= + + + private static final IOModes IOMode = IOModes.REAL; //decides which set of controls to init. +>>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) private static OI instance; private static boolean isHandled = true; private final SmartJoystick mainJoystick; @@ -48,6 +56,7 @@ private OI() { initRealButtons(); break; case AMIR: + initAmirButtons(); //todo do i really need to explain } @@ -63,7 +72,8 @@ public static OI getInstance() { private void initDebugButtons() { // SwerveChassis.getInstance().resetAllEncodersByValues(); // SwerveChassis.getInstance().resetChassisAngle(0); - SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(mainJoystick, true)); + + SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement( true)); mainJoystick.Y.whenPressed(new SwerveCommand() { @Override @@ -92,7 +102,11 @@ public boolean isFinished() { // mainJoystick.Y.whileHeld(new RunFunnel()); // mainJoystick.A.whileHeld(new RunRoller()); // mainJoystick.B.whenPressed(new ToggleRoller()); - + mainJoystick.X.whileHeld(new ShooterByRPM(2000)); + mainJoystick.Y.whileHeld(new RunFunnel()); + mainJoystick.A.whileHeld(new RunRoller()); + mainJoystick.B.whenPressed(new ToggleRoller()); + // mainJoystick.L1.whenPressed(new ParallelCommandGroup( // new CalibrateMaxMin(0.2, SwerveChassis.Module.FRONT_RIGHT), // new CalibrateMaxMin(0.2, SwerveChassis.Module.FRONT_LEFT), @@ -104,12 +118,19 @@ public boolean isFinished() { } private void initRealButtons() { + SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(true)); mainJoystick.A.whileHeld(new FindLocation()); + mainJoystick.B.whenHeld(new CombineJoystickMovement(true, ()-> 0.3)); + mainJoystick.Y.whileHeld(new MoveByVisionSupplier(true)); } private void initAmirButtons() { //Indexing.getInstance().setDefaultCommand(new HandleBalls()); +<<<<<<< HEAD SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(mainJoystick, false )); +======= + SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(false)); +>>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) mainJoystick.Y.whenPressed(new SwerveCommand() { @Override public void initialize() { @@ -123,6 +144,7 @@ public boolean isFinished() { } }); + mainJoystick.POV_UP.whenPressed(new GBCommand() { //todo use instantCommand and dont have buttons disable proper control @Override public void initialize() { diff --git a/src/main/java/edu/greenblitz/pegasus/RobotMap.java b/src/main/java/edu/greenblitz/pegasus/RobotMap.java index 0dcd9c0..4932e50 100644 --- a/src/main/java/edu/greenblitz/pegasus/RobotMap.java +++ b/src/main/java/edu/greenblitz/pegasus/RobotMap.java @@ -166,7 +166,7 @@ public static class Module4 {//back left public static final int lampryID = 1; public static final int MIN_LAMPREY_VAL = 20; public static final int MAX_LAMPREY_VAL = 2646; //todo calibrate in 5v - public static final boolean INVERTED = true; + public static final boolean INVERTED = false; } } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java index 0f1329b..139d2cd 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java @@ -13,8 +13,12 @@ public FindLocation(){ @Override public void initialize() { + System.out.println(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv")); System.out.println((Limelight.getInstance().getLocation())); + + //System.out.println((Limelight.getInstance().getLocation())); + } @Override diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index 18be094..6cdbe36 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -1,39 +1,60 @@ package edu.greenblitz.pegasus.commands.swerve; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; +import edu.greenblitz.pegasus.OI; import edu.greenblitz.pegasus.RobotMap; +<<<<<<< HEAD +======= +import edu.greenblitz.pegasus.subsystems.Limelight; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +>>>>>>> 283ad96 (Romy & Amir - fixed conflicts, please rebase more often) import java.util.function.DoubleSupplier; public class CombineJoystickMovement extends SwerveCommand { - + private DoubleSupplier angSupplier; static double ANG_SPEED_FACTOR = 5;//todo magic number static double LIN_SPEED_FACTOR = RobotMap.Pegasus.Swerve.MAX_VELOCITY; - - public final SmartJoystick joystick; private boolean isSlow; - - public CombineJoystickMovement(SmartJoystick joystick, boolean isSlow) { - this.joystick = joystick; + + public CombineJoystickMovement(boolean isSlow, DoubleSupplier angSupplier){ this.isSlow = isSlow; if (isSlow) { ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes LIN_SPEED_FACTOR *= 0.5; } + this.angSupplier = angSupplier; + } + public CombineJoystickMovement(boolean isSlow){ + this(isSlow, ()->OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.RIGHT_X)); + } + + @Override + public void initialize() { + if (isSlow) { + ANG_SPEED_FACTOR *= 0.5; //todo querry from robot map in initialize to prevent repeated changes + LIN_SPEED_FACTOR *= 0.5; + } } public void execute() { - double leftwardSpeed = -joystick.getAxisValue(SmartJoystick.Axis.LEFT_X) * LIN_SPEED_FACTOR; - double forwardSpeed = joystick.getAxisValue(SmartJoystick.Axis.LEFT_Y) * LIN_SPEED_FACTOR; - double angSpeed = joystick.getAxisValue(SmartJoystick.Axis.RIGHT_X) * ANG_SPEED_FACTOR; - if (forwardSpeed == 0 && leftwardSpeed == 0 && angSpeed == 0) { + double rightwardSpeed = -OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.RIGHT_X)* LIN_SPEED_FACTOR; + double forwardSpeed = OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.LEFT_Y) * LIN_SPEED_FACTOR; + double angSpeed = angSupplier.getAsDouble() * ANG_SPEED_FACTOR; + if (forwardSpeed == 0 && rightwardSpeed == 0 && angSpeed == 0) { swerve.stop(); return; } - swerve.moveByChassisSpeeds(forwardSpeed, leftwardSpeed, angSpeed, - swerve.getChassisAngle() ); + swerve.MoveByChassisSpeeds(forwardSpeed, rightwardSpeed, angSpeed, + -swerve.getChassisAngle()); } + @Override + public void end(boolean interrupted) { + super.end(interrupted); + swerve.stop(); + } } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveByVisionSupplier.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveByVisionSupplier.java new file mode 100644 index 0000000..c26c895 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveByVisionSupplier.java @@ -0,0 +1,11 @@ +package edu.greenblitz.pegasus.commands.swerve; + +import edu.greenblitz.pegasus.subsystems.Limelight; + +import java.util.function.DoubleSupplier; + +public class MoveByVisionSupplier extends CombineJoystickMovement { + public MoveByVisionSupplier(boolean isSlow) { + super(isSlow, ()->Limelight.getInstance().getLocation()); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java deleted file mode 100644 index 3c78d33..0000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java +++ /dev/null @@ -1,10 +0,0 @@ -package edu.greenblitz.pegasus.commands.swerve; - -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; - -public class MoveToTarget extends GBCommand { - public MoveToTarget(double targetAngle){ - SwerveChassis.getInstance().moveChassisLin(); - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java index 9f4e853..f51c87d 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -21,13 +21,18 @@ public static Limelight getInstance() { return instance; } - public double[] getLocation() { + + public double getLocation() { double[] arr = {-1, -1}; + double[] location = new double[2]; + double angle = 1; NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("llpython"); if (loc != null) { System.out.println(Arrays.toString(loc.getDoubleArray(arr))); + location = loc.getDoubleArray(arr); + angle = Math.atan2(location[0],location[1]); } - return new double[5]; + return angle; } } From 5f1e1b6ef1bfabcd1027269382ebd0655f68c2d7 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 31 Oct 2022 18:59:23 +0200 Subject: [PATCH 05/89] noam & tal - fixed all of the list's problems of the swerve --- .../pegasus/commands/swerve/CombineJoystickMovement.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index 6cdbe36..5fe6424 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -3,12 +3,9 @@ import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; import edu.greenblitz.pegasus.OI; import edu.greenblitz.pegasus.RobotMap; -<<<<<<< HEAD -======= import edu.greenblitz.pegasus.subsystems.Limelight; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; ->>>>>>> 283ad96 (Romy & Amir - fixed conflicts, please rebase more often) import java.util.function.DoubleSupplier; @@ -41,14 +38,14 @@ public void initialize() { } public void execute() { - double rightwardSpeed = -OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.RIGHT_X)* LIN_SPEED_FACTOR; + double leftwardSpeed = -OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.RIGHT_X)* LIN_SPEED_FACTOR; double forwardSpeed = OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.LEFT_Y) * LIN_SPEED_FACTOR; double angSpeed = angSupplier.getAsDouble() * ANG_SPEED_FACTOR; - if (forwardSpeed == 0 && rightwardSpeed == 0 && angSpeed == 0) { + if (forwardSpeed == 0 && leftwardSpeed == 0 && angSpeed == 0) { swerve.stop(); return; } - swerve.MoveByChassisSpeeds(forwardSpeed, rightwardSpeed, angSpeed, + swerve.MoveByChassisSpeeds(forwardSpeed, leftwardSpeed, angSpeed, -swerve.getChassisAngle()); } From 8063b47c0fd871b576a87efeb1fa51d0bf081a6e Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 31 Oct 2022 18:59:23 +0200 Subject: [PATCH 06/89] noam & tal - fixed all of the list's problems of the swerve --- src/main/java/edu/greenblitz/pegasus/OI.java | 1 - .../pegasus/commands/swerve/CombineJoystickMovement.java | 8 ++++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index ecd2dfd..80c9e54 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -56,7 +56,6 @@ private OI() { initRealButtons(); break; case AMIR: - initAmirButtons(); //todo do i really need to explain } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index 5fe6424..37532cf 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -31,10 +31,10 @@ public CombineJoystickMovement(boolean isSlow){ @Override public void initialize() { - if (isSlow) { - ANG_SPEED_FACTOR *= 0.5; //todo querry from robot map in initialize to prevent repeated changes - LIN_SPEED_FACTOR *= 0.5; - } + if (isSlow) { + ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes + LIN_SPEED_FACTOR *= 0.5; + } } public void execute() { From 44f0759e9d0805e561eb8b7462cfce5cca99bc02 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 30 Oct 2022 17:54:23 +0200 Subject: [PATCH 07/89] Amir - added getLocation command --- src/main/java/edu/greenblitz/pegasus/OI.java | 2 ++ .../pegasus/commands/shooter/getLocation.java | 8 ++++++++ .../greenblitz/pegasus/subsystems/Limelight.java | 15 +++------------ 3 files changed, 13 insertions(+), 12 deletions(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 80c9e54..ff339ab 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -14,6 +14,7 @@ import edu.greenblitz.pegasus.commands.shooter.FlipShooter; import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; +import edu.greenblitz.pegasus.commands.shooter.getLocation; import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement; import edu.greenblitz.pegasus.commands.swerve.MoveByVisionSupplier; import edu.greenblitz.pegasus.commands.swerve.SwerveCommand; @@ -121,6 +122,7 @@ private void initRealButtons() { mainJoystick.A.whileHeld(new FindLocation()); mainJoystick.B.whenHeld(new CombineJoystickMovement(true, ()-> 0.3)); mainJoystick.Y.whileHeld(new MoveByVisionSupplier(true)); + } private void initAmirButtons() { diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java new file mode 100644 index 0000000..6780741 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java @@ -0,0 +1,8 @@ +package edu.greenblitz.pegasus.commands.shooter; + +public class getLocation extends ShooterCommand{ + @Override + public void initialize() { + super.initialize(); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java index f51c87d..0b135d8 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -4,8 +4,6 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import java.util.Arrays; -import java.util.function.Supplier; public class Limelight extends GBSubsystem { private static Limelight instance; @@ -23,18 +21,11 @@ public static Limelight getInstance() { public double getLocation() { - double[] arr = {-1, -1}; - double[] location = new double[2]; - double angle = 1; - NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("llpython"); - if (loc != null) { - System.out.println(Arrays.toString(loc.getDoubleArray(arr))); - location = loc.getDoubleArray(arr); - angle = Math.atan2(location[0],location[1]); - } + double arr = -1; + NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("angle"); + double angle = loc.getDouble(1); return angle; } } - From 1c99297316932fb06f31f76519656e579441b160 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 30 Oct 2022 20:09:06 +0200 Subject: [PATCH 08/89] Amir - added getlocation function --- src/main/java/edu/greenblitz/pegasus/OI.java | 3 +-- .../pegasus/commands/shooter/getLocation.java | 8 -------- .../pegasus/subsystems/Limelight.java | 18 ++++++++++++++++++ 3 files changed, 19 insertions(+), 10 deletions(-) delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index ff339ab..c9e014e 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -14,7 +14,6 @@ import edu.greenblitz.pegasus.commands.shooter.FlipShooter; import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; -import edu.greenblitz.pegasus.commands.shooter.getLocation; import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement; import edu.greenblitz.pegasus.commands.swerve.MoveByVisionSupplier; import edu.greenblitz.pegasus.commands.swerve.SwerveCommand; @@ -37,7 +36,6 @@ public enum IOModes { private static final IOModes IOMode = IOModes.AMIR; //decides which set of controls to init. ======= - private static final IOModes IOMode = IOModes.REAL; //decides which set of controls to init. >>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) private static OI instance; @@ -123,6 +121,7 @@ private void initRealButtons() { mainJoystick.B.whenHeld(new CombineJoystickMovement(true, ()-> 0.3)); mainJoystick.Y.whileHeld(new MoveByVisionSupplier(true)); + } private void initAmirButtons() { diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java deleted file mode 100644 index 6780741..0000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/shooter/getLocation.java +++ /dev/null @@ -1,8 +0,0 @@ -package edu.greenblitz.pegasus.commands.shooter; - -public class getLocation extends ShooterCommand{ - @Override - public void initialize() { - super.initialize(); - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java index 0b135d8..0b8fe75 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -4,6 +4,11 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +<<<<<<< HEAD +======= +import java.util.Arrays; +import java.util.function.Supplier; +>>>>>>> 7fbcedf (Amir - added getlocation function) public class Limelight extends GBSubsystem { private static Limelight instance; @@ -19,6 +24,7 @@ public static Limelight getInstance() { return instance; } +<<<<<<< HEAD public double getLocation() { double arr = -1; @@ -29,3 +35,15 @@ public double getLocation() { } +======= + public double[] getLocation() { + double[] arr = {-1, -1}; + NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("llpython"); + if (loc != null) { + System.out.println(Arrays.toString(loc.getDoubleArray(arr))); + } + return new double[5]; + } + + } +>>>>>>> 7fbcedf (Amir - added getlocation function) From cfe57c7d6e3903255f06995fbbc2542ad8215b89 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 30 Oct 2022 21:42:25 +0200 Subject: [PATCH 09/89] Romy & Amir - made combineJoystickMovment by supplier --- .../swerve/CombineJoystickMovement.java | 3 ++- .../pegasus/commands/swerve/MoveToTarget.java | 10 +++++++++ .../pegasus/subsystems/Limelight.java | 21 ++++--------------- 3 files changed, 16 insertions(+), 18 deletions(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index 37532cf..df2cac1 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -13,7 +13,7 @@ public class CombineJoystickMovement extends SwerveCommand { private DoubleSupplier angSupplier; static double ANG_SPEED_FACTOR = 5;//todo magic number static double LIN_SPEED_FACTOR = RobotMap.Pegasus.Swerve.MAX_VELOCITY; - + private boolean isSlow; public CombineJoystickMovement(boolean isSlow, DoubleSupplier angSupplier){ @@ -34,6 +34,7 @@ public void initialize() { if (isSlow) { ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes LIN_SPEED_FACTOR *= 0.5; + } } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java new file mode 100644 index 0000000..3c78d33 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java @@ -0,0 +1,10 @@ +package edu.greenblitz.pegasus.commands.swerve; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; + +public class MoveToTarget extends GBCommand { + public MoveToTarget(double targetAngle){ + SwerveChassis.getInstance().moveChassisLin(); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java index 0b8fe75..5a0bae8 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Limelight.java @@ -4,11 +4,7 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -<<<<<<< HEAD -======= -import java.util.Arrays; -import java.util.function.Supplier; ->>>>>>> 7fbcedf (Amir - added getlocation function) + public class Limelight extends GBSubsystem { private static Limelight instance; @@ -24,7 +20,6 @@ public static Limelight getInstance() { return instance; } -<<<<<<< HEAD public double getLocation() { double arr = -1; @@ -35,15 +30,7 @@ public double getLocation() { } -======= - public double[] getLocation() { - double[] arr = {-1, -1}; - NetworkTableEntry loc = NetworkTableInstance.getDefault().getTable("limelight").getEntry("llpython"); - if (loc != null) { - System.out.println(Arrays.toString(loc.getDoubleArray(arr))); - } - return new double[5]; - } - } ->>>>>>> 7fbcedf (Amir - added getlocation function) + + + From b9a390556e864191a883cd239077a0896cd531fe Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 2 Nov 2022 19:45:44 +0200 Subject: [PATCH 10/89] Romy & Asaf - made comand for CombineJoystickMovment by vision supplier UNFINISHED --- src/main/java/edu/greenblitz/pegasus/OI.java | 7 +-- .../commands/shooter/FindLocation.java | 1 + .../swerve/CombineJoystickMovement.java | 1 + .../commands/swerve/MoveBySupplier.java | 52 ------------------- .../pegasus/commands/swerve/MoveToTarget.java | 10 ---- 5 files changed, 4 insertions(+), 67 deletions(-) delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index c9e014e..6b0dbf8 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -73,6 +73,7 @@ private void initDebugButtons() { SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement( true)); + mainJoystick.Y.whenPressed(new SwerveCommand() { @Override public void initialize() { @@ -100,6 +101,7 @@ public boolean isFinished() { // mainJoystick.Y.whileHeld(new RunFunnel()); // mainJoystick.A.whileHeld(new RunRoller()); // mainJoystick.B.whenPressed(new ToggleRoller()); + mainJoystick.X.whileHeld(new ShooterByRPM(2000)); mainJoystick.Y.whileHeld(new RunFunnel()); mainJoystick.A.whileHeld(new RunRoller()); @@ -121,16 +123,11 @@ private void initRealButtons() { mainJoystick.B.whenHeld(new CombineJoystickMovement(true, ()-> 0.3)); mainJoystick.Y.whileHeld(new MoveByVisionSupplier(true)); - } private void initAmirButtons() { //Indexing.getInstance().setDefaultCommand(new HandleBalls()); -<<<<<<< HEAD - SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(mainJoystick, false )); -======= SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(false)); ->>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) mainJoystick.Y.whenPressed(new SwerveCommand() { @Override public void initialize() { diff --git a/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java b/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java index 139d2cd..6f81cd2 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/shooter/FindLocation.java @@ -19,6 +19,7 @@ public void initialize() { //System.out.println((Limelight.getInstance().getLocation())); + } @Override diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index df2cac1..ddc29ee 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -35,6 +35,7 @@ public void initialize() { ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes LIN_SPEED_FACTOR *= 0.5; + } } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java deleted file mode 100644 index 0397330..0000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveBySupplier.java +++ /dev/null @@ -1,52 +0,0 @@ -package edu.greenblitz.pegasus.commands.swerve; - -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; -import edu.greenblitz.pegasus.RobotMap; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -public class MoveBySupplier extends SwerveCommand { - - //we mean left, people are dumb - static double ANG_SPEED_FACTOR = 5;//todo magic number - static double LIN_SPEED_FACTOR = RobotMap.Pegasus.Swerve.MAX_VELOCITY; - DoubleSupplier forward; - DoubleSupplier right; - DoubleSupplier ang; - - public final SmartJoystick joystick; - - private boolean isSlow; - - //supplier returns value 0-1 - public MoveBySupplier(SmartJoystick joystick, boolean isSlow, DoubleSupplier forward, DoubleSupplier right, DoubleSupplier ang) { - this.joystick = joystick; - this.isSlow = isSlow; - this.forward = forward; - this.right = right; - this.ang = ang; - } - - @Override - public void initialize() { - if (isSlow) { - ANG_SPEED_FACTOR *= 0.5; //todo querry from robot map in initialize to prevent repeated changes - LIN_SPEED_FACTOR *= 0.5; - } - } - - public void execute() { - double rightwardSpeed = -right.getAsDouble() * LIN_SPEED_FACTOR; - double forwardSpeed = forward.getAsDouble() * LIN_SPEED_FACTOR; - double angSpeed = ang.getAsDouble() * ANG_SPEED_FACTOR; - if (forwardSpeed == 0 && rightwardSpeed == 0 && angSpeed == 0) { - swerve.stop(); - return; - } - swerve.MoveByChassisSpeeds(forwardSpeed, rightwardSpeed, angSpeed, - -swerve.getChassisAngle() ); - }; - -} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java deleted file mode 100644 index 3c78d33..0000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/MoveToTarget.java +++ /dev/null @@ -1,10 +0,0 @@ -package edu.greenblitz.pegasus.commands.swerve; - -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; - -public class MoveToTarget extends GBCommand { - public MoveToTarget(double targetAngle){ - SwerveChassis.getInstance().moveChassisLin(); - } -} From 0b152e3dea38f4e2d2f8cb2d283739cab465db27 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 2 Nov 2022 20:23:47 +0200 Subject: [PATCH 11/89] Romy - some git conflicts --- src/main/java/edu/greenblitz/pegasus/OI.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 6b0dbf8..fbdeeac 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -101,7 +101,6 @@ public boolean isFinished() { // mainJoystick.Y.whileHeld(new RunFunnel()); // mainJoystick.A.whileHeld(new RunRoller()); // mainJoystick.B.whenPressed(new ToggleRoller()); - mainJoystick.X.whileHeld(new ShooterByRPM(2000)); mainJoystick.Y.whileHeld(new RunFunnel()); mainJoystick.A.whileHeld(new RunRoller()); From a3f5949497fa104d9a69385b7e5382e9d87f8185 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Wed, 9 Nov 2022 15:06:46 +0200 Subject: [PATCH 12/89] Amir & Asaf - commited for one lower case m --- .../pegasus/commands/swerve/CombineJoystickMovement.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index ddc29ee..bbca2f9 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -47,7 +47,7 @@ public void execute() { swerve.stop(); return; } - swerve.MoveByChassisSpeeds(forwardSpeed, leftwardSpeed, angSpeed, + swerve.moveByChassisSpeeds(forwardSpeed, leftwardSpeed, angSpeed, -swerve.getChassisAngle()); } From 8e42748ae3b7c43fcd195d5b892b531c71b84e1a Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 14 Nov 2022 21:23:27 +0200 Subject: [PATCH 13/89] Romy & Amir - added AngPIDSupplier NOT TESTED --- src/main/java/edu/greenblitz/GBLib | 2 +- src/main/java/edu/greenblitz/pegasus/OI.java | 37 +++++++------------ .../java/edu/greenblitz/pegasus/RobotMap.java | 9 ++--- .../commands/swerve/AngPIDSupplier.java | 24 ++++++++++++ .../swerve/CombineJoystickMovement.java | 11 +----- .../commands/swerve/RotateToAngle.java | 13 ------- 6 files changed, 45 insertions(+), 51 deletions(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/swerve/RotateToAngle.java diff --git a/src/main/java/edu/greenblitz/GBLib b/src/main/java/edu/greenblitz/GBLib index d7ba1fc..457063c 160000 --- a/src/main/java/edu/greenblitz/GBLib +++ b/src/main/java/edu/greenblitz/GBLib @@ -1 +1 @@ -Subproject commit d7ba1fc464ce1f36e7f00ce5ecec3e239a064685 +Subproject commit 457063c60d45d5c57bb658b9a80fff03eace7da5 diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index fbdeeac..3a35cd4 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -14,16 +14,14 @@ import edu.greenblitz.pegasus.commands.shooter.FlipShooter; import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; +import edu.greenblitz.pegasus.commands.swerve.AngPIDSupplier; import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement; import edu.greenblitz.pegasus.commands.swerve.MoveByVisionSupplier; import edu.greenblitz.pegasus.commands.swerve.SwerveCommand; import edu.greenblitz.pegasus.commands.swerve.garbage.CalibrateMaxMin; -<<<<<<< HEAD import edu.greenblitz.pegasus.subsystems.Indexing; import edu.greenblitz.pegasus.utils.DigitalInputMap; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -======= ->>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; public class OI { @@ -31,13 +29,7 @@ public class OI { public enum IOModes { DEBUG, REAL, AMIR } -<<<<<<< HEAD - private static final IOModes IOMode = IOModes.AMIR; //decides which set of controls to init. -======= - - private static final IOModes IOMode = IOModes.REAL; //decides which set of controls to init. ->>>>>>> b672a60 (Romy & Amir - fixed conflicts, please rebase more often) private static OI instance; private static boolean isHandled = true; private final SmartJoystick mainJoystick; @@ -127,19 +119,19 @@ private void initRealButtons() { private void initAmirButtons() { //Indexing.getInstance().setDefaultCommand(new HandleBalls()); SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(false)); - mainJoystick.Y.whenPressed(new SwerveCommand() { - @Override - public void initialize() { - swerve.resetChassisAngle(); - SmartDashboard.putNumber("pigeon",swerve.getChassisAngle()); - } - - @Override - public boolean isFinished() { - return true; - } - }); - +// mainJoystick.Y.whenPressed(new SwerveCommand() { +// @Override +// public void initialize() { +// swerve.resetChassisAngle(); +// SmartDashboard.putNumber("pigeon",swerve.getChassisAngle()); +// } +// +// @Override +// public boolean isFinished() { +// return true; +// } +// }); + mainJoystick.Y.whileHeld(new CombineJoystickMovement(true, new AngPIDSupplier(() -> 0.3))); mainJoystick.POV_UP.whenPressed(new GBCommand() { //todo use instantCommand and dont have buttons disable proper control @Override @@ -189,7 +181,6 @@ public void end(boolean interrupted) { secondJoystick.POV_UP.whenPressed(new ShooterEvacuate()); } - public SmartJoystick getMainJoystick() { return mainJoystick; } diff --git a/src/main/java/edu/greenblitz/pegasus/RobotMap.java b/src/main/java/edu/greenblitz/pegasus/RobotMap.java index 4932e50..75a975e 100644 --- a/src/main/java/edu/greenblitz/pegasus/RobotMap.java +++ b/src/main/java/edu/greenblitz/pegasus/RobotMap.java @@ -118,8 +118,7 @@ public static class Swerve { public static final PIDObject angPID = new PIDObject().withKp(0.5).withKd(10).withMaxPower(0.8); public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5); - public static final PIDObject rotationPID = new PIDObject().withKp(0.5).withKi(0).withKd(0).withFF(0.1); - + public static final PIDObject rotationPID = new PIDObject().withKp(0.1).withKi(0).withKd(0).withFF(0.1); public static final double ks = 0.14876; public static final double kv = 3.3055; @@ -137,7 +136,7 @@ public static class Module1 {//front right public static final int lampryID = 2; public static final int MIN_LAMPREY_VAL = 12; public static final int MAX_LAMPREY_VAL = 4041; - public static final boolean INVERTED = false; + public static final boolean INVERTED = true; } public static class Module2 {//front left @@ -147,7 +146,7 @@ public static class Module2 {//front left public static final int MIN_LAMPREY_VAL = 22; public static final int MAX_LAMPREY_VAL = 4040; - public static final boolean INVERTED = true; + public static final boolean INVERTED = false; } public static class Module3 {//back right @@ -157,7 +156,7 @@ public static class Module3 {//back right public static final int MIN_LAMPREY_VAL = 32; public static final int MAX_LAMPREY_VAL = 4021; - public static final boolean INVERTED = false; + public static final boolean INVERTED = true; } public static class Module4 {//back left diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java new file mode 100644 index 0000000..7c434e1 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java @@ -0,0 +1,24 @@ +package edu.greenblitz.pegasus.commands.swerve; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; +import edu.greenblitz.pegasus.RobotMap; +import edu.wpi.first.math.controller.PIDController; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class AngPIDSupplier implements DoubleSupplier{ + private DoubleSupplier ang; + private PIDController chassisPid; + + public AngPIDSupplier(DoubleSupplier ang){ + this.ang = ang; + chassisPid = new PIDController(RobotMap.Pegasus.Swerve.rotationPID.getKp(),RobotMap.Pegasus.Swerve.rotationPID.getKi(),RobotMap.Pegasus.Swerve.rotationPID.getKd()); + } + + @Override + public double getAsDouble() { + return chassisPid.calculate(SwerveChassis.getInstance().getChassisAngle(),ang.getAsDouble()); + + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java index bbca2f9..49dba0d 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/CombineJoystickMovement.java @@ -18,11 +18,6 @@ public class CombineJoystickMovement extends SwerveCommand { public CombineJoystickMovement(boolean isSlow, DoubleSupplier angSupplier){ this.isSlow = isSlow; - if (isSlow) { - ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes - LIN_SPEED_FACTOR *= 0.5; - - } this.angSupplier = angSupplier; } public CombineJoystickMovement(boolean isSlow){ @@ -34,13 +29,11 @@ public void initialize() { if (isSlow) { ANG_SPEED_FACTOR *= 0.8; //todo querry from robot map in initialize to prevent repeated changes LIN_SPEED_FACTOR *= 0.5; - - } } public void execute() { - double leftwardSpeed = -OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.RIGHT_X)* LIN_SPEED_FACTOR; + double leftwardSpeed = -OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.LEFT_X) * LIN_SPEED_FACTOR; double forwardSpeed = OI.getInstance().getMainJoystick().getAxisValue(SmartJoystick.Axis.LEFT_Y) * LIN_SPEED_FACTOR; double angSpeed = angSupplier.getAsDouble() * ANG_SPEED_FACTOR; if (forwardSpeed == 0 && leftwardSpeed == 0 && angSpeed == 0) { @@ -48,7 +41,7 @@ public void execute() { return; } swerve.moveByChassisSpeeds(forwardSpeed, leftwardSpeed, angSpeed, - -swerve.getChassisAngle()); + swerve.getChassisAngle()); } @Override diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/RotateToAngle.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/RotateToAngle.java deleted file mode 100644 index b25a42d..0000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/RotateToAngle.java +++ /dev/null @@ -1,13 +0,0 @@ -package edu.greenblitz.pegasus.commands.swerve; - -public class RotateToAngle extends SwerveCommand{//todo delete - private double ang; - - public RotateToAngle(double angInRads){ - this.ang = angInRads; - } - - public void execute(){ - swerve.moveChassisLin(ang,0); - } -} From a3f8fda74097f57b6da99bccaf42a24ae2fc3bd7 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Wed, 16 Nov 2022 21:16:41 +0200 Subject: [PATCH 14/89] Romy & Amir & Heinemann - CombineJoystickMovment by supplier works TESTED --- src/main/java/edu/greenblitz/pegasus/OI.java | 19 ++++++++++++++++++- .../java/edu/greenblitz/pegasus/Robot.java | 2 ++ .../commands/swerve/AngPIDSupplier.java | 14 +++++++++++--- 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 3a35cd4..d79cd17 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -1,6 +1,7 @@ package edu.greenblitz.pegasus; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.gyro.PigeonGyro; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.hid.SmartJoystick; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; import edu.greenblitz.pegasus.commands.funnel.RunFunnel; @@ -23,6 +24,7 @@ import edu.greenblitz.pegasus.utils.DigitalInputMap; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import org.opencv.core.Mat; public class OI { @@ -131,7 +133,10 @@ private void initAmirButtons() { // return true; // } // }); - mainJoystick.Y.whileHeld(new CombineJoystickMovement(true, new AngPIDSupplier(() -> 0.3))); + SmartDashboard.putNumber("ang target", 0); + mainJoystick.Y.whileHeld(new CombineJoystickMovement(false, new AngPIDSupplier(()-> SmartDashboard.getNumber("ang target", 0)))); + SmartDashboard.putNumber("ang in deg", Math.toDegrees(SmartDashboard.getNumber("ang target", 0))); + SmartDashboard.putNumber("ang dif", SwerveChassis.getInstance().getChassisAngle() - Math.toDegrees(SmartDashboard.getNumber("ang target", 0))); mainJoystick.POV_UP.whenPressed(new GBCommand() { //todo use instantCommand and dont have buttons disable proper control @Override @@ -146,6 +151,18 @@ public boolean isFinished() { } }); + mainJoystick.B.whenPressed(new SwerveCommand() { + @Override + public void initialize() { + swerve.resetChassisAngle(); + } + + @Override + public boolean isFinished() { + return true; + } + }); + mainJoystick.R1.whileHeld(new GBCommand() { //todo make whenHeld possibly interruptible=false @Override public void initialize() { diff --git a/src/main/java/edu/greenblitz/pegasus/Robot.java b/src/main/java/edu/greenblitz/pegasus/Robot.java index c45dbc9..8a42fb2 100644 --- a/src/main/java/edu/greenblitz/pegasus/Robot.java +++ b/src/main/java/edu/greenblitz/pegasus/Robot.java @@ -132,6 +132,8 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putNumber("pigeon angle", Math.toDegrees(SwerveChassis.getInstance().getChassisAngle())); + SmartDashboard.putNumber("ang in deg", Math.toDegrees(SmartDashboard.getNumber("ang target", 0))); + SmartDashboard.putNumber("ang dif", Math.toDegrees(SwerveChassis.getInstance().getChassisAngle() - SmartDashboard.getNumber("ang target", 0))); } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java index 7c434e1..b529921 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java @@ -3,6 +3,7 @@ import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; import edu.greenblitz.pegasus.RobotMap; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -12,13 +13,20 @@ public class AngPIDSupplier implements DoubleSupplier{ private PIDController chassisPid; public AngPIDSupplier(DoubleSupplier ang){ + SmartDashboard.putNumber("kp", 0.1); + SmartDashboard.putNumber("ki", 0); + SmartDashboard.putNumber("kd", 0); this.ang = ang; chassisPid = new PIDController(RobotMap.Pegasus.Swerve.rotationPID.getKp(),RobotMap.Pegasus.Swerve.rotationPID.getKi(),RobotMap.Pegasus.Swerve.rotationPID.getKd()); + chassisPid.enableContinuousInput(0,2*Math.PI); //min and max } @Override public double getAsDouble() { - return chassisPid.calculate(SwerveChassis.getInstance().getChassisAngle(),ang.getAsDouble()); - + double kp = SmartDashboard.getNumber("kp",0.1); + double kd = SmartDashboard.getNumber("kd",0); + double ki = SmartDashboard.getNumber("ki",0); + chassisPid.setPID(kp,ki,kd); + return Math.max(Math.min(chassisPid.calculate(SwerveChassis.getInstance().getChassisAngle(),ang.getAsDouble()),0.4),-0.4); } -} +} \ No newline at end of file From 32ed6d15d7551fb103a112b7862c1b670d487256 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Wed, 16 Nov 2022 21:18:33 +0200 Subject: [PATCH 15/89] added new line --- .../edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java b/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java index b529921..f98d7d4 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/swerve/AngPIDSupplier.java @@ -29,4 +29,4 @@ public double getAsDouble() { chassisPid.setPID(kp,ki,kd); return Math.max(Math.min(chassisPid.calculate(SwerveChassis.getInstance().getChassisAngle(),ang.getAsDouble()),0.4),-0.4); } -} \ No newline at end of file +} From 75ff53f9ddb9223b4fc460f260ca2fd40b2a8557 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 20 Nov 2022 18:58:08 +0200 Subject: [PATCH 16/89] noam -added battery subsystem --- src/main/java/edu/greenblitz/GBLib | 2 +- src/main/java/edu/greenblitz/pegasus/OI.java | 1 + .../java/edu/greenblitz/pegasus/Robot.java | 6 ++++ .../pegasus/commands/BatteryDisabler.java | 32 +++++++++++++++++++ .../pegasus/subsystems/Battery.java | 30 +++++++++++++++++ 5 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java create mode 100644 src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java diff --git a/src/main/java/edu/greenblitz/GBLib b/src/main/java/edu/greenblitz/GBLib index d7ba1fc..457063c 160000 --- a/src/main/java/edu/greenblitz/GBLib +++ b/src/main/java/edu/greenblitz/GBLib @@ -1 +1 @@ -Subproject commit d7ba1fc464ce1f36e7f00ce5ecec3e239a064685 +Subproject commit 457063c60d45d5c57bb658b9a80fff03eace7da5 diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index 154b8c1..e36f23d 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -56,6 +56,7 @@ public static OI getInstance() { if (instance == null) { instance = new OI(); } + return instance; } diff --git a/src/main/java/edu/greenblitz/pegasus/Robot.java b/src/main/java/edu/greenblitz/pegasus/Robot.java index d4c7047..ef78d1a 100644 --- a/src/main/java/edu/greenblitz/pegasus/Robot.java +++ b/src/main/java/edu/greenblitz/pegasus/Robot.java @@ -8,10 +8,12 @@ import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.shooter.Shooter; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveChassis; import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.swerve.SwerveModule; +import edu.greenblitz.pegasus.commands.BatteryDisabler; import edu.greenblitz.pegasus.commands.auto.Taxi; import edu.greenblitz.pegasus.commands.intake.extender.ToggleRoller; import edu.greenblitz.pegasus.commands.multiSystem.InsertIntoShooter; import edu.greenblitz.pegasus.commands.shooter.ShooterByRPM; +import edu.greenblitz.pegasus.subsystems.Battery; import edu.greenblitz.pegasus.subsystems.Dashboard; import edu.greenblitz.pegasus.subsystems.Indexing; @@ -36,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import java.util.ArrayList; +import java.util.Base64; public class Robot extends TimedRobot { @Override @@ -126,6 +129,8 @@ public void robotInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putNumber("pigeon angle", Math.toDegrees(SwerveChassis.getInstance().getChassisAngle())); + + } @@ -133,6 +138,7 @@ public void robotPeriodic() { public void disabledInit() { //VisionMaster.GameState.DISABLED.setAsCurrent(); CommandScheduler.getInstance().cancelAll(); + Battery.getInstance().setDefaultCommand(new BatteryDisabler()); } @Override diff --git a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java new file mode 100644 index 0000000..5d1c0c2 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java @@ -0,0 +1,32 @@ +package edu.greenblitz.pegasus.commands; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.pegasus.subsystems.Battery; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +public class BatteryDisabler extends GBCommand { + + private boolean under; + private Battery battery; + + public BatteryDisabler (){ + battery = new Battery(); + require(battery); + } + + @Override + public void initialize() { + under = false; + } + + @Override + public void execute() { + if(battery.getCurrentVoltage() < battery.getMinVoltage()){ + under = true; + } + if (under){ + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().disable(); + } + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java new file mode 100644 index 0000000..00b82e8 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java @@ -0,0 +1,30 @@ +package edu.greenblitz.pegasus.subsystems; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.GBSubsystem; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; + +import java.sql.Driver; + +public class Battery extends GBSubsystem { + + private double currentVoltage; + private static final double minVoltage = 11.97; + private static Battery instance; + + + public static Battery getInstance(){ + if(instance == null){ + instance = new Battery(); + } + return instance; + } + + public double getCurrentVoltage() { + return RobotController.getBatteryVoltage(); + } + + public double getMinVoltage(){ + return minVoltage; + } +} From 17e3b057447a249aded4d5fdf40201718897d77a Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 21 Nov 2022 11:03:57 +0200 Subject: [PATCH 17/89] added a tick timer until disables the robot when the battery goes brrrrrr --- .../pegasus/commands/BatteryDisabler.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java index 5d1c0c2..2ad8262 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java @@ -2,12 +2,15 @@ import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; import edu.greenblitz.pegasus.subsystems.Battery; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.CommandScheduler; public class BatteryDisabler extends GBCommand { - private boolean under; private Battery battery; + public static final int disableAfterTicks = 5; + public int timesUnder; + public BatteryDisabler (){ battery = new Battery(); @@ -16,15 +19,18 @@ public BatteryDisabler (){ @Override public void initialize() { - under = false; + timesUnder = 0; + CommandScheduler.getInstance().enable(); //todo - is this need? } @Override public void execute() { if(battery.getCurrentVoltage() < battery.getMinVoltage()){ - under = true; + timesUnder++; } - if (under){ + if (timesUnder >= disableAfterTicks && + DriverStation.getMatchType() == DriverStation.MatchType.None || + DriverStation.getMatchType() == DriverStation.MatchType.Practice){ CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().disable(); } From 7c5f1d9ad03e8c44fef4fb7b9c51b0255c9a8a52 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 21 Nov 2022 16:28:20 +0200 Subject: [PATCH 18/89] noam - fixed some stuff --- .../edu/greenblitz/pegasus/commands/BatteryDisabler.java | 7 +++---- .../java/edu/greenblitz/pegasus/subsystems/Battery.java | 1 + 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java index 2ad8262..56bec00 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java @@ -13,14 +13,13 @@ public class BatteryDisabler extends GBCommand { public BatteryDisabler (){ - battery = new Battery(); + battery = Battery.getInstance(); require(battery); } @Override public void initialize() { timesUnder = 0; - CommandScheduler.getInstance().enable(); //todo - is this need? } @Override @@ -29,8 +28,8 @@ public void execute() { timesUnder++; } if (timesUnder >= disableAfterTicks && - DriverStation.getMatchType() == DriverStation.MatchType.None || - DriverStation.getMatchType() == DriverStation.MatchType.Practice){ + (DriverStation.getMatchType() == DriverStation.MatchType.None || + DriverStation.getMatchType() == DriverStation.MatchType.Practice)){ CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().disable(); } diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java index 00b82e8..626ac01 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java @@ -12,6 +12,7 @@ public class Battery extends GBSubsystem { private static final double minVoltage = 11.97; private static Battery instance; + private Battery (){} public static Battery getInstance(){ if(instance == null){ From 8c3743cd4cb482770519931a0db52d81eaf7cdd5 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Wed, 23 Nov 2022 20:57:04 +0200 Subject: [PATCH 19/89] noam - now periodic runs in a if the battery not dead --- src/main/java/edu/greenblitz/pegasus/Robot.java | 7 +++++-- .../edu/greenblitz/pegasus/commands/BatteryDisabler.java | 2 ++ .../java/edu/greenblitz/pegasus/subsystems/Battery.java | 2 ++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/edu/greenblitz/pegasus/Robot.java b/src/main/java/edu/greenblitz/pegasus/Robot.java index ef78d1a..d98d0bd 100644 --- a/src/main/java/edu/greenblitz/pegasus/Robot.java +++ b/src/main/java/edu/greenblitz/pegasus/Robot.java @@ -127,10 +127,13 @@ public void robotInit() { @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); - SmartDashboard.putNumber("pigeon angle", Math.toDegrees(SwerveChassis.getInstance().getChassisAngle())); + //TODO noam - because of the low battery disable command everything if the periodic must be in the if + if(!Battery.isBatteryLow){ + CommandScheduler.getInstance().run(); + SmartDashboard.putNumber("pigeon angle", Math.toDegrees(SwerveChassis.getInstance().getChassisAngle())); + } } diff --git a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java index 56bec00..782a55c 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java @@ -13,6 +13,7 @@ public class BatteryDisabler extends GBCommand { public BatteryDisabler (){ + Battery.isBatteryLow = false; battery = Battery.getInstance(); require(battery); } @@ -30,6 +31,7 @@ public void execute() { if (timesUnder >= disableAfterTicks && (DriverStation.getMatchType() == DriverStation.MatchType.None || DriverStation.getMatchType() == DriverStation.MatchType.Practice)){ + Battery.isBatteryLow = true; CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().disable(); } diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java index 626ac01..fff00cb 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Battery.java @@ -11,6 +11,8 @@ public class Battery extends GBSubsystem { private double currentVoltage; private static final double minVoltage = 11.97; private static Battery instance; + public static boolean isBatteryLow = false; + private Battery (){} From 4fe93591310b024e53339bc566adbbc8416c4d7b Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Thu, 24 Nov 2022 14:36:54 +0200 Subject: [PATCH 20/89] now supposed to disable robot --- .../edu/greenblitz/pegasus/commands/BatteryDisabler.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java index 782a55c..2d1bb51 100644 --- a/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java +++ b/src/main/java/edu/greenblitz/pegasus/commands/BatteryDisabler.java @@ -31,9 +31,12 @@ public void execute() { if (timesUnder >= disableAfterTicks && (DriverStation.getMatchType() == DriverStation.MatchType.None || DriverStation.getMatchType() == DriverStation.MatchType.Practice)){ - Battery.isBatteryLow = true; + CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().disable(); + + Battery.isBatteryLow = true; + DriverStation.inDisabled(true); } } } From 6e1186699ecb3d9113f7c2539416d2ed289bd1d6 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Sun, 27 Nov 2022 20:52:09 +0200 Subject: [PATCH 21/89] Amir & Romy & Asaf - added combinejoystick TESTED not working --- .idea/jarRepositories.xml | 10 +++++ src/main/java/edu/greenblitz/pegasus/OI.java | 32 ++++++++++++++- .../java/edu/greenblitz/pegasus/Robot.java | 12 +++--- .../commands/shooter/FindLocation.java | 2 +- .../commands/swerve/MoveByVisionSupplier.java | 2 +- .../pegasus/subsystems/Dashboard.java | 7 ++-- .../pegasus/subsystems/Limelight.java | 32 +++++++++------ vendordeps/PhotonLib-json-1.0.json | 41 +++++++++++++++++++ 8 files changed, 113 insertions(+), 25 deletions(-) create mode 100644 vendordeps/PhotonLib-json-1.0.json diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml index 4f6473e..4a6c7db 100644 --- a/.idea/jarRepositories.xml +++ b/.idea/jarRepositories.xml @@ -106,5 +106,15 @@