diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 04b921b1..c34b496d 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -52,12 +52,19 @@ public static final class Effectorc { public static final int INTAKE = 0; public static final int OUTTAKE = 1; // 0.0001184 - public static final double[] kP = { 0, 0 /* 0.030717,0.0001 */ }; + + /* + * public static final double kP = 0.0001067; public static final double kI = 0; public + * static final double kD = 0; public static final double kS = 0; public static final double + * kV = 0.11124; public static final double kA = 0.039757; + */ + + public static final double[] kP = {0, 0.0001067}; public static final double[] kI = { /* /Intake/ */0, /* /Outake/ */0 }; public static final double[] kD = { /* /Intake/ */0, /* /Outake/ */0 }; public static final double[] kS = { /* /Intake/ */0.22, /* /Outake/ */0.29753 * 2 }; - public static final double[] kV = { 0.122, 0/* 0.065239, 0.077913 */ }; - public static final double[] kA = { 0, 0/* 0.0062809,0.05289 */ }; + public static final double[] kV = {0.122, 0.11124}; + public static final double[] kA = {0, 0.039757}; public static final int INTAKE_PORT = 9; // port public static final int OUTTAKE_PORT = 10; // port public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port @@ -173,7 +180,8 @@ public static final class Armc { // Boundaries public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder - public static final double POS_TOLERANCE_RAD = Units.degreesToRadians(5); // placeholder //Whether or not this is the actual + public static final double POS_TOLERANCE_RAD = + Units.degreesToRadians(5); // placeholder //Whether or not this is the actual // account // idk TODO: test on actual encoder without a conversion // factor @@ -386,6 +394,7 @@ public static final class Limelightc { // to represent unreliable heading public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity + public static final double ERROR_TOLERANCE_RAD = 0.1; // unused public static final double HORIZONTAL_FOV_DEG = 0; // unused public static final double RESOLUTION_WIDTH_PIX = 640; // unused diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 2ed786db..be54556c 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -209,7 +209,9 @@ private void setBindingsManipulator() { //new JoystickButton(manipulatorController, A_BUTTON) //.onTrue(new RampMaxRPMDriving(intakeShooter)); - + axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( + new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), + new PassToOuttake(intakeShooter))); new JoystickButton(manipulatorController, RAMP_OUTTAKE) .whileTrue(new RampMaxRPM(intakeShooter)); @@ -221,8 +223,8 @@ private void setBindingsManipulator() { ? new TestRPM(intakeShooter) : new Outtake(intakeShooter, arm)*/ - axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - .onTrue(new PassToOuttake(intakeShooter)); + // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) + // .onTrue(new PassToOuttake(intakeShooter)); axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) .whileTrue(new Intake(intakeShooter)); @@ -241,6 +243,7 @@ private void setBindingsManipulator() { new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm)); new POVButton(manipulatorController, LEFT_D_PAD) .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD)); + } diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java index 085ac066..331c04f1 100644 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java @@ -19,10 +19,11 @@ public AimArmSpeaker(Arm arm, Limelight ll) { this.ll = ll; } + // Called when the command is initially scheduled. @Override public void initialize() { - double goal = ll.getArmAngleToShootSpeakerRad(); + double goal = ll.getOptimizedArmAngleRadsMT2(); arm.setArmTarget(goal); } diff --git a/src/main/java/org/carlmontrobotics/commands/Intake.java b/src/main/java/org/carlmontrobotics/commands/Intake.java index 7321bb5f..344757c4 100644 --- a/src/main/java/org/carlmontrobotics/commands/Intake.java +++ b/src/main/java/org/carlmontrobotics/commands/Intake.java @@ -12,7 +12,7 @@ public class Intake extends Command { // intake until sees game peice or 4sec has passed private final IntakeShooter intake; private int index = 0; - private double increaseSpeed = .01; + private double increaseSpeed = .007; private double initSpeed = .5; private double slowSpeed = .1;