diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 3249a1b..cce05c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -17,7 +17,7 @@ public class ServoPositions { public static double transferServo_in = 0.4; - public static double hoodDefault = 0.46; + public static double hoodDefault = 0.38; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index cb5a03e..7bb38ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -20,6 +20,8 @@ import java.util.Objects; public class Shooter implements Subsystem { private final DcMotorEx fly1; private final DcMotorEx fly2; + + private final DcMotorEx encoder; private final Servo hoodServo; private final Servo turret1; @@ -76,6 +78,8 @@ public class Shooter implements Subsystem { this.turret2 = robot.turr2; + this.encoder = robot.shooterEncoder; + @@ -266,6 +270,8 @@ public class Shooter implements Subsystem { } + + public void setTelemetryOn(boolean state){telemetryOn = state;} public void moveTurret(double pos){ @@ -290,9 +296,16 @@ public class Shooter implements Subsystem { fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + fly1.setVelocity(velocity); + + fly2.setPower(fly1.getPower()); + + + + + + - fly1.setVelocity(velocity, AngleUnit.DEGREES); - fly2.setVelocity(velocity, AngleUnit.DEGREES); } else if (Objects.equals(shooterMode, "POS")){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 1f0828d..27f3af5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -13,6 +13,7 @@ import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; @@ -97,7 +98,7 @@ public class TeleopV1 extends LinearOpMode { MecanumDrive drive ; - public boolean autotrack = true; + public boolean autotrack = false; public int last = 0; public int second = 0; @@ -260,6 +261,8 @@ public class TeleopV1 extends LinearOpMode { } + + intake(); drivetrain.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 5c6055e..0887ace 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -86,6 +87,7 @@ public class ShooterTest extends LinearOpMode { TELE.update(); + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 19a779e..cbae6f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -66,6 +66,8 @@ public class Robot { public WebcamName webcam; + public DcMotorEx shooterEncoder; + @@ -146,5 +148,7 @@ public class Robot { + + } }