summ more fixed before match 5
This commit is contained in:
@@ -17,7 +17,7 @@ public class ServoPositions {
|
|||||||
|
|
||||||
public static double transferServo_in = 0.4;
|
public static double transferServo_in = 0.4;
|
||||||
|
|
||||||
public static double hoodDefault = 0.46;
|
public static double hoodDefault = 0.38;
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -20,6 +20,8 @@ import java.util.Objects;
|
|||||||
public class Shooter implements Subsystem {
|
public class Shooter implements Subsystem {
|
||||||
private final DcMotorEx fly1;
|
private final DcMotorEx fly1;
|
||||||
private final DcMotorEx fly2;
|
private final DcMotorEx fly2;
|
||||||
|
|
||||||
|
private final DcMotorEx encoder;
|
||||||
private final Servo hoodServo;
|
private final Servo hoodServo;
|
||||||
|
|
||||||
private final Servo turret1;
|
private final Servo turret1;
|
||||||
@@ -76,6 +78,8 @@ public class Shooter implements Subsystem {
|
|||||||
|
|
||||||
this.turret2 = robot.turr2;
|
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 setTelemetryOn(boolean state){telemetryOn = state;}
|
||||||
|
|
||||||
public void moveTurret(double pos){
|
public void moveTurret(double pos){
|
||||||
@@ -290,9 +296,16 @@ public class Shooter implements Subsystem {
|
|||||||
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
fly2.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")){
|
else if (Objects.equals(shooterMode, "POS")){
|
||||||
|
|||||||
@@ -13,6 +13,7 @@ import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
|
|||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
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.libs.RR.MecanumDrive;
|
||||||
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
|
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
|
||||||
import org.firstinspires.ftc.teamcode.subsystems.Intake;
|
import org.firstinspires.ftc.teamcode.subsystems.Intake;
|
||||||
@@ -97,7 +98,7 @@ public class TeleopV1 extends LinearOpMode {
|
|||||||
|
|
||||||
MecanumDrive drive ;
|
MecanumDrive drive ;
|
||||||
|
|
||||||
public boolean autotrack = true;
|
public boolean autotrack = false;
|
||||||
|
|
||||||
public int last = 0;
|
public int last = 0;
|
||||||
public int second = 0;
|
public int second = 0;
|
||||||
@@ -260,6 +261,8 @@ public class TeleopV1 extends LinearOpMode {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
intake();
|
intake();
|
||||||
|
|
||||||
drivetrain.update();
|
drivetrain.update();
|
||||||
|
|||||||
@@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
|||||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||||
|
|
||||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||||
@@ -86,6 +87,7 @@ public class ShooterTest extends LinearOpMode {
|
|||||||
TELE.update();
|
TELE.update();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -66,6 +66,8 @@ public class Robot {
|
|||||||
|
|
||||||
public WebcamName webcam;
|
public WebcamName webcam;
|
||||||
|
|
||||||
|
public DcMotorEx shooterEncoder;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -146,5 +148,7 @@ public class Robot {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user