diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index 2848835..865d44e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -1,14 +1,11 @@ package org.firstinspires.ftc.teamcode.utils; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.kP; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.maxStep; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.config.Config; +@Config public class Flywheel { - Robot robot; - MultipleTelemetry TELE; - + public static double kP = 0.001; // small proportional gain (tune this) + public static double maxStep = 0.06; // prevents sudden jumps double initPos = 0.0; double stamp = 0.0; double stamp1 = 0.0; 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 fd781e9..6ba66f8 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 @@ -4,7 +4,9 @@ import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; @@ -30,8 +32,10 @@ public class Robot { limelight3A = hardwareMap.get(Limelight3A.class, "limelight"); shooter1 = hardwareMap.get(DcMotorEx.class, "s1"); - shooter2 = hardwareMap.get(DcMotorEx.class, "s2"); + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); + shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); hood = hardwareMap.get(Servo.class, "hood");