diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index 5d6cc60..1008adf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -227,6 +227,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { } break; case OPENGATE: + robot.setIntakePower(0); if (currentTime - timeStamp > openGateTime){ follower.followPath(openGate_shoot1, true); pathState = PathState.DRIVE_SHOOT1; @@ -275,7 +276,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { break; case DRIVE_PICKUP3: if (!follower.isBusy()){ - shooter.setFlywheelVelocity(2200); + shooter.setFlywheelVelocity(2250); robot.setHoodPos(0.75); follower.followPath(drivePickup3_pickup3, intakePower, false); pathState = PathState.PICKUP3; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 76638a3..41a9a5a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -8,6 +8,7 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; @@ -254,9 +255,9 @@ public class TeleopV4 extends LinearOpMode { } if (gamepad2.dpadUpWasPressed()){ - hoodOffset+=0.02; - } else if (gamepad2.dpadDownWasPressed()){ hoodOffset-=0.02; + } else if (gamepad2.dpadDownWasPressed()){ + hoodOffset+=0.02; } if (gamepad2.dpadRightWasPressed()){ @@ -284,6 +285,16 @@ public class TeleopV4 extends LinearOpMode { TELE.addData("Velocity 2", flywheel.getVelo2()); TELE.addData("Flywheel Offset", flywheelOffset); TELE.addData("Hood Offset", hoodOffset); + + TELE.addData("FR Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS)); + TELE.addData("FL Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS)); + TELE.addData("BL Drive", robot.backLeft.getCurrent(CurrentUnit.AMPS)); + TELE.addData("BR Drive", robot.backRight.getCurrent(CurrentUnit.AMPS)); + + TELE.addData("Flywheel 1", robot.shooter1.getCurrent(CurrentUnit.AMPS)); + TELE.addData("Flywheel 2", robot.shooter2.getCurrent(CurrentUnit.AMPS)); + TELE.addData("Transfer", robot.transfer.getCurrent(CurrentUnit.AMPS)); + TELE.addData("Intake", robot.intake.getCurrent(CurrentUnit.AMPS)); // // TELE.addData("Current Position", currentPose); // diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 6a3c35f..8936737 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -138,8 +138,8 @@ public class Shooter { ); flywheelVelocity = commander.getPredictedRPM(); - - robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset); + double hood1 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset)); + robot.setHoodPos(hood1); fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset); fly.setF(voltage); break; @@ -181,7 +181,8 @@ public class Shooter { ); flywheelVelocity = commander.getPredictedRPM(); - robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset); + double hood2 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset)); + robot.setHoodPos(hood2); fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset); break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index 4526bbc..b868ea9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -53,7 +53,7 @@ public class SpindexerTransferIntake { } int[] shootOrder = {0, 1, 2}; - public static final double sensorDistanceThreshold = 4.75;//4.85//5.35 + public static final double sensorDistanceThreshold = 6.0;//4.85//5.35 final long pulseTime = 70; // ms private DesiredPattern desiredPattern = DesiredPattern.GPP; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index 241e079..340a649 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -17,8 +17,8 @@ public class Turret { private final double servoTicksPer180 = 0.58; public static double neutralPosition = 0.51; - private final double turretMin = 0.08; - private final double turretMax = 0.91; + private final double turretMin = 0.13; + private final double turretMax = 0.87; public static boolean limelightUsed = true; public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005; LLResult result;