diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java
index 0121dcf..b8e88e1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/KronBot.java
@@ -2,38 +2,33 @@
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KD;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KF;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KI;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KP;
-
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
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 org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.kronbot.utils.drivers.MotorDriver;
import org.firstinspires.ftc.teamcode.kronbot.utils.wrappers.ControlHubGyroscope;
import org.firstinspires.ftc.teamcode.kronbot.utils.wrappers.Servo;
-import com.qualcomm.robotcore.hardware.ColorSensor;
-
+/**
+ * Container class that holds references for all electronic hardware on the robot
+ */
public class KronBot {
+ /** Deprecated, is always null. Use PedroPathing follower instead. */
public MotorDriver motors;
+ // Use this instead
+ public Follower follower = null;
public ControlHubGyroscope gyroscope;
- public ModernRoboticsI2cRangeSensor rangeSensor;
-
- public DcMotorEx intakeMotor, leftOuttake, rightOuttake;
- public Servo loaderServo;
-
- public Servo turretServo, angleServo, flapsServo;
- public ColorSensor outtakeColor;
-
+ public DcMotorEx intakeMotor, leftOuttake, rightOuttake, loaderMotor;
+ public Servo loaderServo, turretServo, angleServo, flapsServo;
+ /*
public void initDrivetrain(HardwareMap hardwareMap) {
DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
@@ -44,38 +39,27 @@ public void initDrivetrain(HardwareMap hardwareMap) {
motors = new MotorDriver();
motors.init(leftRear, leftFront, rightRear, rightFront);
}
- public void initMotors(HardwareMap hardwareMap) {
+ */
+ public void initMotors(HardwareMap hardwareMap) {
intakeMotor = hardwareMap.get(DcMotorEx.class, "intakeMotor");
+ loaderMotor = hardwareMap.get(DcMotorEx.class, "loaderMotor");
+
leftOuttake = hardwareMap.get(DcMotorEx.class, "shooter0");
leftOuttake.setDirection(DcMotorSimple.Direction.REVERSE);
- leftOuttake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- leftOuttake.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ leftOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
leftOuttake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- leftOuttake.setVelocityPIDFCoefficients(
- OUT_MOTOR_KP, // P - main stabilizer
- OUT_MOTOR_KI, // I - usually 0
- OUT_MOTOR_KD, // D - reduces overshoot
- OUT_MOTOR_KF // F - feedforward (VERY important)
- );
+
rightOuttake = hardwareMap.get(DcMotorEx.class, "shooter1");
rightOuttake.setDirection(DcMotorSimple.Direction.REVERSE);
- rightOuttake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- rightOuttake.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightOuttake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- rightOuttake.setVelocityPIDFCoefficients(
- OUT_MOTOR_KP, // P - main stabilizer
- OUT_MOTOR_KI, // I - usually 0
- OUT_MOTOR_KD, // D - reduces overshoot
- OUT_MOTOR_KF // F - feedforward (VERY important)
- );
-
}
public void initServos(HardwareMap hardwareMap) {
loaderServo = new Servo(hardwareMap);
- loaderServo.init("loader", true, false, 0, 0, 0);
+ loaderServo.init("loader", true, true, 0, 0, 0);
loaderServo.runContinuous(false, false);
turretServo = new Servo(hardwareMap);
turretServo.init("turretPivot", false, false, 0, 1, 0.5);
@@ -87,35 +71,44 @@ public void initServos(HardwareMap hardwareMap) {
public void initSensors(HardwareMap hardwareMap) {
//outtakeColor = hardwareMap.get(ColorSensor.class, "outtakeColor");
- rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "rangeSensor");
}
-
-
-
- public void initAutonomy(HardwareMap hardwareMap) {
- initMotors(hardwareMap);
- initServos(hardwareMap);
- initSensors(hardwareMap);
+ /**
+ * Initialize PedroPathing follower
+ * @param hardwareMap Used to initialize drivetrain and localizers
+ * @param loadPose If true, will try to load a pose from the file system (if it fails, it loads a 0 pose)
+ */
+ public void initFollower(HardwareMap hardwareMap, boolean loadPose) {
+ follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap);
+ if(loadPose) {
+ Pose startingPose = PoseStorage.loadPose();
+ follower.setStartingPose(startingPose);
+ follower.update();
+ }
+ else
+ follower.setStartingPose(new Pose());
}
-
- public void initTeleop(HardwareMap hardwareMap) {
- initMotors(hardwareMap);
- initDrivetrain(hardwareMap);
-
- //initIMU2(hardwareMap);
- initServos(hardwareMap);
- initSensors(hardwareMap);
+ /**
+ * Initialize PedroPathing follower with a 0 starting pose
+ * @param hardwareMap Used to initialize drivetrain and localizers
+ */
+ public void initFollower(HardwareMap hardwareMap) { initFollower(hardwareMap, false); }
+
+ /**
+ * Initialize PedroPathing follower with a starting pose
+ * @param hardwareMap Used to initialize drivetrain and localizers
+ * @param startingPose The starting pose
+ */
+ public void initFollower(HardwareMap hardwareMap, Pose startingPose) {
+ follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap);
+ follower.setStartingPose(startingPose);
}
- public void initSimpleDriving(HardwareMap hardwareMap) {
- //initIMU2(hardwareMap);
- initMotors(hardwareMap);
- }
- public void init(HardwareMap hardwareMap){
+ public void initHardware(HardwareMap hardwareMap) {
+ if(follower != null)
+ initFollower(hardwareMap);
initMotors(hardwareMap);
- //initIMU2(hardwareMap);
initServos(hardwareMap);
initSensors(hardwareMap);
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java
index 8cf5d88..101f6e0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java
@@ -1,33 +1,22 @@
package org.firstinspires.ftc.teamcode.kronbot;
-import com.pedropathing.follower.Follower;
-import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.kronbot.utils.detection.AprilTagWebcam;
-import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad1;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_FAR;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MAX;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MIN;
+import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.DELTA_THRESHOLD;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.LOADER_SERVO_REVERSED;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KD;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KF;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KI;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KP;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_P;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_S;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_V;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_USE_PID;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1_ANGLE;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1_KS;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_1_VELOCITY;
@@ -49,18 +38,20 @@
public class Robot extends KronBot {
// Singleton instance
private static Robot instance = null;
-
+
// Systems used in all opModes
public AprilTagWebcam webcam = new AprilTagWebcam();
+
public final Outtake outtake;
public final Intake intake;
public final Loader loader;
public final Turret turret;
public final Flap flap;
public final Shoot shoot;
- public Follower follower;
+ public final Heading heading;
+
+
-
// Private constructor
public Robot() {
this.outtake = new Outtake();
@@ -69,8 +60,9 @@ public Robot() {
this.turret = new Turret();
this.shoot = new Shoot();
this.flap = new Flap();
+ this.heading = new Heading();
}
-
+
// Get the singleton instance
public static Robot getInstance() {
if (instance == null) {
@@ -78,46 +70,57 @@ public static Robot getInstance() {
}
return instance;
}
-
+
// Initialize robot and all systems
public void init(HardwareMap hardwareMap) {
- super.init(hardwareMap);
+ super.initHardware(hardwareMap);
initSystems(hardwareMap);
}
-
+
public void initSystems(HardwareMap hardwareMap) {
+ if(follower == null)
+ initFollower(hardwareMap);
+ follower.update();
outtake.init();
intake.init();
loader.init();
turret.init();
flap.init();
+ heading.init();
+
+ //Add other inits here
- //Add other intis here
- Pose startingPose = PoseStorage.loadPose();
- follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap);
- follower.setStartingPose(startingPose);
}
-
+
// Updates all systems
+ // Except pedro
public void updateAllSystems() {
outtake.update();
intake.update();
loader.update();
turret.update();
flap.update();
+ follower.update();
+
+ double rawHeading = follower.getHeading();
+ heading.update(rawHeading);
+ double filtered = heading.get();
+ /**
+ eg usage for turret calculations:
+ double robotRelativeAngle = fieldRelativeAngle - filtered;
+ */
// gyroscope.updateOrientation();
//Add other updates here
// webcam.update();
- if(follower != null)
- follower.update();
}
public class Outtake {
public boolean on = false;
public double angle = 0;
+ public double autoAimAngle = 0;
public double velocity;
public double kS = 0;
public boolean reversed = false;
@@ -131,6 +134,8 @@ public void init() {
velocity = minVelocity;
}
+
+
/** Configures the launch angle and launch motor speed for the given distance.
* Returns true if a good configuration is possible (If the distance is in the correct range)
* @param distance The distance, measured horizontally, from the tower wall to the center of the turret.
@@ -163,65 +168,30 @@ else if(distance > 250 && distance < 375)
return true;
}
- double p, i, d, f;
public void update(){
- if(p != OUT_MOTOR_KP || i != OUT_MOTOR_KI || d != OUT_MOTOR_KD || f != OUT_MOTOR_KF) {
- p = OUT_MOTOR_KP;
- d = OUT_MOTOR_KD;
- i = OUT_MOTOR_KI;
- f = OUT_MOTOR_KF;
-
- leftOuttake.setVelocityPIDFCoefficients(
- OUT_MOTOR_KP, // P - main stabilizer
- OUT_MOTOR_KI, // I - usually 0
- OUT_MOTOR_KD, // D - reduces overshoot
- OUT_MOTOR_KF // F - feedforward (VERY important)
- );
- rightOuttake.setVelocityPIDFCoefficients(
- OUT_MOTOR_KP, // P - main stabilizer
- OUT_MOTOR_KI, // I - usually 0
- OUT_MOTOR_KD, // D - reduces overshoot
- OUT_MOTOR_KF // F - feedforward (VERY important)
- );
-
- }
-
- if(lastVelocity > velocity) {
- braking = true;
- }
- lastVelocity = velocity;
-
-
if(on){
- if(OUT_USE_PID) {
+ leftOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ rightOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ if(leftOuttake.getVelocity() < velocity * 1) {
leftOuttake.setPower(1);
- leftOuttake.setVelocity(velocity);
rightOuttake.setPower(1);
- rightOuttake.setVelocity(velocity);
- } else {
- leftOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- rightOuttake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- if(leftOuttake.getVelocity() < velocity * 1) {
- leftOuttake.setPower(1);
- rightOuttake.setPower(1);
- }
- else if(leftOuttake.getVelocity() > velocity * 1.1) {
- if(braking) {
- leftOuttake.setPower(0);
- rightOuttake.setPower(0);
- }
- leftOuttake.setPower(kS * 0.8);
- rightOuttake.setPower(kS * 0.8);
- }
- else {
- braking = false;
- leftOuttake.setPower(kS);
- rightOuttake.setPower(kS);
+ }
+ else if(leftOuttake.getVelocity() > velocity * 1.1) {
+ if(braking) {
+ leftOuttake.setPower(0);
+ rightOuttake.setPower(0);
}
-
+ leftOuttake.setPower(kS * 0.8);
+ rightOuttake.setPower(kS * 0.8);
+ }
+ else {
+ braking = false;
+ leftOuttake.setPower(kS);
+ rightOuttake.setPower(kS);
}
+
} else {
if(leftOuttake.getVelocity() < 21) {
leftOuttake.setPower(0);
@@ -288,7 +258,6 @@ public void telemetry(Telemetry telemetry) {
telemetry.addData("Right Power", "%.3f", rightOuttake.getPower());
telemetry.addData("Angle", "%.3f", angle);
telemetry.addData("Angle Servo Pos", "%.3f", angleServo.getPosition());
- telemetry.addData("Distance", "%.3f", rangeSensor.cmUltrasonic() * 1.08644 + 17.20917); // magic numbers from desmos
}
}
@@ -323,65 +292,97 @@ public void telemetry(Telemetry telemetry) {
public class Loader {
public double speed;
- public boolean reversed = false;
public void init() {
- loaderServo.setPosition(0.5);
+ loaderMotor.setPower(0);
}
public void update() {
- if(!reversed)
- loaderServo.setPosition((speed + 1) / 2);
- else
- loaderServo.setPosition(1 - ((speed + 1) / 2));
+ loaderMotor.setPower(speed);
}
public void telemetry(Telemetry telemetry) {
telemetry.addLine("=== LOADER STATUS ===");
telemetry.addData("Speed", speed);
- telemetry.addData("Reversed", reversed);
- telemetry.addData("Servo Position", "%.3f", loaderServo.getPosition());
}
}
-
public class Turret {
/** Angle in radians from straight ahead */
public double angle = 0;
public double driverOffset = 0;
private double servoPosition;
+
+ public boolean autoAimEnabled = true;
+ static final double basket_X = 130;
+ static final double basket_Y = 135;
+
public void init() {
angle = 0;
servoPosition = 0.5;
}
public void update() {
- if (turretServo != null && follower != null) {
- if(angle > Math.PI)
- angle = -2 * Math.PI + angle;
- if(angle < -Math.PI)
- angle = 2 * Math.PI + angle;
+ //angle to the basket
+ if (turretServo == null || follower == null) return;
- if(driverOffset > Math.PI)
- driverOffset = -2 * Math.PI + driverOffset;
- if(driverOffset < -Math.PI)
- driverOffset = 2 * Math.PI + driverOffset;
+ if(autoAimEnabled) {
+ double robot_X = follower.getPose().getX();
+ double robot_Y = follower.getPose().getY();
+ double robotHeading =follower.getPose().getHeading();
- double fieldRelativeAngle = angle + driverOffset;
+ double dx = basket_X - robot_X;
+ double dy = basket_Y - robot_Y;
- if(fieldRelativeAngle > Math.PI)
- fieldRelativeAngle = -2 * Math.PI + fieldRelativeAngle;
- if(fieldRelativeAngle < -Math.PI)
- fieldRelativeAngle = 2 * Math.PI + fieldRelativeAngle;
+ double targetFieldAngle = Math.atan2(dy, dx);
- double robotRelativeAngle = fieldRelativeAngle - (follower.getHeading()/* * 0.01745329*/); // deg to radian
- servoPosition = robotRelativeAngle * TURRET_SERVO_UNITS_PER_RAD + 0.5;
+ //calculate
+ double robotRelativeAngle = targetFieldAngle - robotHeading;
+ //normalize
+ robotRelativeAngle = Math.atan2(
+ Math.sin(robotRelativeAngle),
+ Math.cos(robotRelativeAngle)
+ );
- turretServo.setPosition(Math.clamp(servoPosition, TURRET_SERVO_MIN, TURRET_SERVO_MAX));
+ servoPosition = robotRelativeAngle * TURRET_SERVO_UNITS_PER_RAD + 0.5;
+ } else {
+ servoPosition =
+ driverOffset * TURRET_SERVO_UNITS_PER_RAD + 0.5;
}
+
+ turretServo.setPosition(
+ Math.clamp(servoPosition, TURRET_SERVO_MIN, TURRET_SERVO_MAX)
+ );
+
+
+// if (turretServo != null && follower != null) {
+// if(angle > Math.PI)
+// angle = -2 * Math.PI + angle;
+// if(angle < -Math.PI)
+// angle = 2 * Math.PI + angle;
+//
+// if(driverOffset > Math.PI)
+// driverOffset = -2 * Math.PI + driverOffset;
+// if(driverOffset < -Math.PI)
+// driverOffset = 2 * Math.PI + driverOffset;
+//
+// double fieldRelativeAngle = angle + driverOffset;
+//
+//
+// if(fieldRelativeAngle > Math.PI)
+// fieldRelativeAngle = -2 * Math.PI + fieldRelativeAngle;
+// if(fieldRelativeAngle < -Math.PI)
+// fieldRelativeAngle = 2 * Math.PI + fieldRelativeAngle;
+//
+// double robotRelativeAngle = fieldRelativeAngle - (follower.getHeading()/* * 0.01745329*/); // deg to radian
+// servoPosition = robotRelativeAngle * TURRET_SERVO_UNITS_PER_RAD + 0.5;
+//
+//
+// turretServo.setPosition(Math.clamp(servoPosition, TURRET_SERVO_MIN, TURRET_SERVO_MAX));
+// }
}
public void telemetry(Telemetry telemetry) {
@@ -408,6 +409,55 @@ public void update(){
}
}
+
+ public class Heading {
+
+ private double lastRawHeading = 0.0; // last reading from PinPoint
+ private double lastFilteredRate = 0.0; // filtered angular velocity
+ private double filteredHeading = 0.0; // output heading
+ private final double lpAlpha = 0.2; // smoothing factor, (0.05 - 0.2 for tuning?)
+ private final double loopDt = 0.02; // control loop period (sec)
+
+
+ /** Prevents IMU from PinPoint from drifting
+ * Calculates the angular velocity (heading / time) from the PinPoint IMU
+ * Applies a low-pass filter to the velocity — tiny changes caused by vibration we believe?
+ * Integrates the filtered angular velocity back into a stable heading
+ */
+
+ public void init(){
+ filteredHeading = 0;
+ }
+
+ public void update (double rawHeading) {
+ double delta = rawHeading - lastRawHeading;
+
+ //simple threshold filter
+ //if delta larger than threshold, we add it to our filtered heading, if not, we ignore it
+ if(delta>DELTA_THRESHOLD || delta<-DELTA_THRESHOLD) {
+ //wrap filtered heading -PI..PI
+// filteredHeading = Math.atan2(Math.sin(filteredHeading), Math.cos(filteredHeading));
+ filteredHeading += delta;
+ }
+ lastRawHeading = rawHeading;
+ }
+ /**
+ *@return Returns the current filtered heading
+ */
+ public double get() {
+ return filteredHeading;
+ }
+
+ public void telemetry(Telemetry telemetry) {
+ telemetry.addLine("=== HEADING STATUS ===");
+ telemetry.addData("Raw Heading", "%.4f", lastRawHeading);
+ telemetry.addData("Filtered Heading", "%.4f", filteredHeading);
+ telemetry.addData("Filtered Rate", "%.4f rad/s", lastFilteredRate);
+ }
+ }
+
+
+
public class ShootClose {
public void activate() {
@@ -502,7 +552,6 @@ public void telemetry(Telemetry telemetry) {
telemetry.addData("Left Front Power", "%.2f", motors.leftFront.getPower());
telemetry.addData("Right Front Power", "%.2f", motors.rightFront.getPower());
telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
- telemetry.addData("Right Rear Power", "%.2f", motors.rightRear.getPower());
}
}
-}
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java
index 379a67e..30483ec 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackBlueOp.java
@@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.kronbot.autonomous;
import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN;
import static java.lang.Thread.sleep;
@@ -16,15 +15,15 @@
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import org.firstinspires.ftc.teamcode.kronbot.KronBot;
+//import org.firstinspires.ftc.teamcode.kronbot.KronBot;
+import org.firstinspires.ftc.teamcode.kronbot.Robot;
import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@Autonomous(name = "BLUE Auto_Back", group = org.firstinspires.ftc.teamcode.kronbot.utils.Constants.TEST_GROUP)
public class Auto_BackBlueOp extends OpMode {
- private KronBot robot;
- private Follower follower;
+ private Robot robot = Robot.getInstance();
private Timer pathTimer, opmodeTimer;
private int pathState, launchState;
@@ -41,19 +40,21 @@ public class Auto_BackBlueOp extends OpMode {
@Override
public void init() {
- robot = new KronBot();
- robot.initAutonomy(hardwareMap);
+
+ robot.init(hardwareMap);
+
+ robot.initFollower(hardwareMap, startingPoseBack);
pathTimer = new Timer();
opmodeTimer = new Timer();
opmodeTimer.resetTimer();
- follower = Constants.createFollower(hardwareMap);
- FtcDashboard dashboard = FtcDashboard.getInstance();
- telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
+ telemetry = new MultipleTelemetry(
+ telemetry,
+ FtcDashboard.getInstance().getTelemetry()
+ );
buildPaths();
- follower.setStartingPose(startingPoseBack);
telemetry.addLine("Initialized. Waiting for start...");
telemetry.update();
@@ -62,12 +63,12 @@ public void init() {
/** Build all paths for the auto **/
public void buildPaths() {
- goToLaunch = follower.pathBuilder()
+ goToLaunch = robot.follower.pathBuilder()
.addPath(new BezierLine(startingPoseBack, launchZoneBack))
.setLinearHeadingInterpolation(startingPoseBack.getHeading(), launchZoneBack.getHeading())
.build();
- goToPark = follower.pathBuilder()
+ goToPark = robot.follower.pathBuilder()
.addPath(new BezierLine(launchZoneBack, parkBack))
.setLinearHeadingInterpolation(launchZoneBack.getHeading(), parkBack.getHeading())
.build();
@@ -92,7 +93,7 @@ public void start() {
@Override
public void loop() {
- follower.update();
+ robot.follower.update();
motorVel = robot.leftOuttake.getVelocity();
@@ -100,7 +101,7 @@ public void loop() {
- Pose currentPose = follower.getPose();
+ Pose currentPose = robot.follower.getPose();
telemetry.addData("Path State", pathState);
telemetry.addData("X", currentPose.getX());
telemetry.addData("Y", currentPose.getY());
@@ -114,12 +115,12 @@ public void autonomousPathUpdate() {
switch (pathState) {
case 0:
- follower.followPath(goToLaunch);
+ robot.follower.followPath(goToLaunch);
setPathState(1);
break;
case 1:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
switch (launchState) {
case 0:
// Start outtake motors
@@ -201,14 +202,14 @@ public void autonomousPathUpdate() {
case 2:
- if (!follower.isBusy()) {
- follower.followPath(goToPark);
+ if (!robot.follower.isBusy()) {
+ robot.follower.followPath(goToPark);
setPathState(3);
}
break;
case 3:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
setPathState(-1);
}
break;
@@ -216,7 +217,7 @@ public void autonomousPathUpdate() {
case -1:
// Idle / done
- Pose finalPose = follower.getPose();
+ Pose finalPose = robot.follower.getPose();
PoseStorage.savePose(finalPose);
break;
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java
index 154d877..1f07d41 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_BackRedOp.java
@@ -1,14 +1,11 @@
package org.firstinspires.ftc.teamcode.kronbot.autonomous;
import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN;
import static java.lang.Thread.sleep;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
@@ -16,7 +13,6 @@
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import org.firstinspires.ftc.teamcode.kronbot.KronBot;
import org.firstinspires.ftc.teamcode.kronbot.Robot;
import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@@ -24,8 +20,7 @@
@Autonomous(name = "RED Auto_Back", group = org.firstinspires.ftc.teamcode.kronbot.utils.Constants.TEST_GROUP)
public class Auto_BackRedOp extends OpMode {
- private Robot robot;
- private Follower follower;
+ private Robot robot = Robot.getInstance();
private Timer pathTimer, opmodeTimer;
private int pathState, launchState;
@@ -42,19 +37,21 @@ public class Auto_BackRedOp extends OpMode {
@Override
public void init() {
- robot = Robot.getInstance();
- robot.initAutonomy(hardwareMap);
+
+ robot.init(hardwareMap);
+
+ robot.initFollower(hardwareMap, startingPose);
pathTimer = new Timer();
opmodeTimer = new Timer();
opmodeTimer.resetTimer();
- follower = Constants.createFollower(hardwareMap);
- FtcDashboard dashboard = FtcDashboard.getInstance();
- telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
+ telemetry = new MultipleTelemetry(
+ telemetry,
+ FtcDashboard.getInstance().getTelemetry()
+ );
buildPaths();
- follower.setStartingPose(startingPose);
telemetry.addLine("Initialized. Waiting for start...");
telemetry.update();
@@ -63,12 +60,12 @@ public void init() {
public void buildPaths() {
- goToLaunch = follower.pathBuilder()
+ goToLaunch = robot.follower.pathBuilder()
.addPath(new BezierLine(startingPose, launchZone))
.setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading())
.build();
- goToPark = follower.pathBuilder()
+ goToPark = robot.follower.pathBuilder()
.addPath(new BezierLine(launchZone, parkZone))
.setLinearHeadingInterpolation(launchZone.getHeading(), parkZone.getHeading())
.build();
@@ -93,13 +90,13 @@ public void start() {
@Override
public void loop() {
- follower.update();
+ robot.follower.update();
motorVel = robot.leftOuttake.getVelocity();
autonomousPathUpdate();
- Pose currentPose = follower.getPose();
+ Pose currentPose = robot.follower.getPose();
telemetry.addData("Path State", pathState);
telemetry.addData("X", currentPose.getX());
telemetry.addData("Y", currentPose.getY());
@@ -113,12 +110,12 @@ public void autonomousPathUpdate() {
switch (pathState) {
case 0:
- follower.followPath(goToLaunch);
+ robot.follower.followPath(goToLaunch);
setPathState(1);
break;
case 1:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
switch (launchState) {
case 0:
// Start outtake motors
@@ -212,22 +209,22 @@ public void autonomousPathUpdate() {
case 2:
- if (!follower.isBusy()) {
- follower.followPath(goToPark);
+ if (!robot.follower.isBusy()) {
+ robot.follower.followPath(goToPark);
setPathState(3);
}
break;
case 3:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
setPathState(-1);
}
break;
case -1:
- Pose finalPose = follower.getPose();
- PoseStorage.savePose(finalPose);
+// Pose finalPose = robot.follower.getPose();
+// PoseStorage.savePose(finalPose);
break;
}
@@ -241,6 +238,8 @@ public void setPathState(int newState) {
@Override
public void stop() {
+ Pose finalPose = robot.follower.getPose();
+ PoseStorage.savePose(finalPose);
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java
index 4464c73..e8158e2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseBlueOp.java
@@ -1,13 +1,11 @@
package org.firstinspires.ftc.teamcode.kronbot.autonomous;
import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE;
import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
@@ -16,14 +14,14 @@
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.kronbot.KronBot;
+import org.firstinspires.ftc.teamcode.kronbot.Robot;
import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@Autonomous(name = "BLUE Auto_Close", group = org.firstinspires.ftc.teamcode.kronbot.utils.Constants.TEST_GROUP)
public class Auto_CloseBlueOp extends OpMode {
- private KronBot robot;
- private Follower follower;
+ private Robot robot = Robot.getInstance();
private Timer pathTimer, opmodeTimer;
private int pathState;
private int launchState;
@@ -43,19 +41,21 @@ public class Auto_CloseBlueOp extends OpMode {
@Override
public void init() {
- robot = new KronBot();
- robot.initAutonomy(hardwareMap);
+
+ robot.init(hardwareMap);
+
+ robot.initFollower(hardwareMap, startingPose);
pathTimer = new Timer();
opmodeTimer = new Timer();
opmodeTimer.resetTimer();
- follower = Constants.createFollower(hardwareMap);
- FtcDashboard dashboard = FtcDashboard.getInstance();
- telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
+ telemetry = new MultipleTelemetry(
+ telemetry,
+ FtcDashboard.getInstance().getTelemetry()
+ );
buildPaths();
- follower.setStartingPose(startingPose);
telemetry.addLine("Initialized. Waiting for start...");
telemetry.update();
@@ -64,14 +64,14 @@ public void init() {
public void buildPaths() {
- goToLaunch = follower.pathBuilder()
+ goToLaunch = robot.follower.pathBuilder()
.addPath(new BezierLine(startingPose, launchZone))
.setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading())
.addPath(new BezierLine(launchZone, launchZone2))
.setLinearHeadingInterpolation(launchZone.getHeading(), launchZone2.getHeading())
.build();
- goToPark = follower.pathBuilder()
+ goToPark = robot.follower.pathBuilder()
.addPath(new BezierLine(launchZone2, parkZone))
.setLinearHeadingInterpolation(launchZone2.getHeading(), parkZone.getHeading())
.build();
@@ -95,13 +95,13 @@ public void start() {
@Override
public void loop() {
- follower.update();
+ robot.follower.update();
motorVel = robot.leftOuttake.getVelocity();
autonomousPathUpdate();
- Pose currentPose = follower.getPose();
+ Pose currentPose = robot.follower.getPose();
telemetry.addData("Path State", pathState);
telemetry.addData("X", currentPose.getX());
telemetry.addData("Y", currentPose.getY());
@@ -116,12 +116,12 @@ public void autonomousPathUpdate() {
switch (pathState) {
case 0:
- follower.followPath(goToLaunch);
+ robot.follower.followPath(goToLaunch);
setPathState(1);
break;
case 1:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
switch (launchState) {
case 0:
// Start outtake motors
@@ -202,21 +202,21 @@ public void autonomousPathUpdate() {
case 2:
- if (!follower.isBusy()) {
- follower.followPath(goToPark);
+ if (!robot.follower.isBusy()) {
+ robot.follower.followPath(goToPark);
setPathState(3);
}
break;
case 3:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
setPathState(-1);
}
break;
case -1:
- Pose finalPose = follower.getPose();
+ Pose finalPose = robot.follower.getPose();
PoseStorage.savePose(finalPose);
break;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java
index 3dba4b6..8e00891 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/Auto_CloseRedOp.java
@@ -1,14 +1,10 @@
package org.firstinspires.ftc.teamcode.kronbot.autonomous;
import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.*;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.minVelocity;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
@@ -17,14 +13,14 @@
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.kronbot.KronBot;
+import org.firstinspires.ftc.teamcode.kronbot.Robot;
import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@Autonomous(name = "RED Auto_Close", group = org.firstinspires.ftc.teamcode.kronbot.utils.Constants.TEST_GROUP)
public class Auto_CloseRedOp extends OpMode {
- private KronBot robot;
- private Follower follower;
+ private Robot robot = Robot.getInstance();
private Timer pathTimer, opmodeTimer;
private int pathState;
private int launchState;
@@ -43,19 +39,22 @@ public class Auto_CloseRedOp extends OpMode {
@Override
public void init() {
- robot = new KronBot();
- robot.initAutonomy(hardwareMap);
+
+ robot.init(hardwareMap);
+
+
+ robot.initFollower(hardwareMap, startingPose);
pathTimer = new Timer();
opmodeTimer = new Timer();
opmodeTimer.resetTimer();
- follower = Constants.createFollower(hardwareMap);
- FtcDashboard dashboard = FtcDashboard.getInstance();
- telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
+ telemetry = new MultipleTelemetry(
+ telemetry,
+ FtcDashboard.getInstance().getTelemetry()
+ );
buildPaths();
- follower.setStartingPose(startingPose);
telemetry.addLine("Initialized. Waiting for start...");
telemetry.update();
@@ -64,14 +63,14 @@ public void init() {
public void buildPaths() {
- goToLaunch = follower.pathBuilder()
+ goToLaunch = robot.follower.pathBuilder()
.addPath(new BezierLine(startingPose, launchZone))
.setLinearHeadingInterpolation(startingPose.getHeading(), launchZone.getHeading())
.addPath(new BezierLine(launchZone, launchZone2))
.setLinearHeadingInterpolation(launchZone.getHeading(), launchZone2.getHeading())
.build();
- goToPark = follower.pathBuilder()
+ goToPark = robot.follower.pathBuilder()
.addPath(new BezierLine(startingPose, parkZone))
.setLinearHeadingInterpolation(startingPose.getHeading(), parkZone.getHeading())
.build();
@@ -95,13 +94,13 @@ public void start() {
@Override
public void loop() {
- follower.update();
+ robot.follower.update();
motorVel = robot.leftOuttake.getVelocity();
autonomousPathUpdate();
- Pose currentPose = follower.getPose();
+ Pose currentPose = robot.follower.getPose();
telemetry.addData("Path State", pathState);
telemetry.addData("X", currentPose.getX());
telemetry.addData("Y", currentPose.getY());
@@ -116,13 +115,13 @@ public void autonomousPathUpdate() {
switch (pathState) {
case 0:
- follower.followPath(goToPark);
+ robot.follower.followPath(goToPark);
setPathState(-1);
break;
case 1:
/*
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
switch (launchState) {
case 0:
// Start outtake motors
@@ -202,21 +201,21 @@ public void autonomousPathUpdate() {
*/
case 2:
- if (!follower.isBusy()) {
- follower.followPath(goToPark);
+ if (!robot.follower.isBusy()) {
+ robot.follower.followPath(goToPark);
setPathState(3);
}
break;
case 3:
- if (!follower.isBusy()) {
+ if (!robot.follower.isBusy()) {
setPathState(-1);
}
break;
case -1:
- Pose finalPose = follower.getPose();
+ Pose finalPose = robot.follower.getPose();
PoseStorage.savePose(finalPose);
break;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java
index 10482d1..ec22518 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java
@@ -28,9 +28,9 @@ public Coordinates(double x, double y, double heading) {
public static Coordinates LaunchZoneClose = new Coordinates(-25, 32, -0.8);
public static Coordinates LaunchZoneClose2 = new Coordinates(-19, 55, -0.84);
public static Coordinates ParkClose = new Coordinates(-20, 18, 0);
- public static Coordinates StartingPoseBackRed = new Coordinates(0, 0, 0);
- public static Coordinates LaunchZoneBack = new Coordinates(13, -2.3, -0.4);
- public static Coordinates ParkBack = new Coordinates(30, 0, 0);
+ public static Coordinates StartingPoseBackRed = new Coordinates(79, 7.4, 0);
+ public static Coordinates LaunchZoneBack = new Coordinates(83, 19, -0.4);
+ public static Coordinates ParkBack = new Coordinates(81, 35, 0);
/// BLUE auto movement
public static Coordinates StartingPoseCloseBlue = new Coordinates(0, 0, 0);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/IdleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/IdleOp.java
index 0e8ff6b..48e4716 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/IdleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/IdleOp.java
@@ -11,7 +11,7 @@ public class IdleOp extends LinearOpMode {
KronBot robot = new KronBot();
@Override
public void runOpMode() throws InterruptedException {
- robot.initAutonomy(hardwareMap);
+ robot.initHardware(hardwareMap);
while (!isStopRequested() && opModeIsActive()) {
telemetry.update();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/BaseAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/BaseAuto.java
new file mode 100644
index 0000000..f13d022
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/BaseAuto.java
@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.kronbot.baseOps;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.pedropathing.paths.PathChain;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Function;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.kronbot.Robot;
+import org.firstinspires.ftc.teamcode.kronbot.utils.misc.LpsCounter;
+
+public abstract class BaseAuto extends OpMode {
+ Robot robot;
+
+ FtcDashboard dashboard;
+
+ LpsCounter lpsCounter;
+
+
+ @Override
+ public void init() {
+ lpsCounter.getLoopTime();
+
+ robot = Robot.getInstance();
+ robot.initSystems(hardwareMap);
+ robot.initFollower(hardwareMap);
+
+ try {
+ robot.follower.getPoseTracker().getLocalizer().resetIMU();
+ } catch (InterruptedException e) {
+ throw new RuntimeException(e);
+ }
+
+ autoTelemetry();
+ }
+
+ @Override
+ public void init_loop() {
+
+ }
+
+ @Override
+ public void start() {
+
+ }
+
+ @Override
+ public void loop() {
+
+ }
+
+ @Override
+ public void stop() {
+
+ }
+
+ void autoTelemetry() {
+ telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta);
+
+ robot.intake.telemetry(telemetry);
+ robot.loader.telemetry(telemetry);
+ robot.outtake.telemetry(telemetry);
+ robot.turret.telemetry(telemetry);
+
+ telemetry.update();
+ }
+
+ public void followPath(PathChain path) {
+
+ }
+
+ public void runIntake(double speed) {
+
+ }
+
+ public void runOuttake(double time, double speed, double angle) {
+
+ }
+
+ public void runOuttake(double time) {
+
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/MainTeleOp.java
index 650f901..687f5ed 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/MainTeleOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/baseOps/MainTeleOp.java
@@ -28,15 +28,13 @@ public class MainTeleOp extends OpMode {
private FtcDashboard dashboard;
- Button reverseButton = new Button();
-
boolean isLaunching = false;
boolean wasRightBumperPressed = false;
double currentVelocity = 1300;
public void init()
{
- robot.initTeleop(hardwareMap);
+ robot.initHardware(hardwareMap);
dashboard = FtcDashboard.getInstance();
telemetry.update();
@@ -124,4 +122,4 @@ public void stop()
aprilTagWebcam.stop();
}
-}
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/IdleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/IdleOp.java
deleted file mode 100644
index ae6515d..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/IdleOp.java
+++ /dev/null
@@ -1,48 +0,0 @@
-package org.firstinspires.ftc.teamcode.kronbot.manual;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.teamcode.kronbot.KronBot;
-import org.firstinspires.ftc.teamcode.kronbot.utils.components.FieldCentricDrive;
-import org.firstinspires.ftc.teamcode.kronbot.utils.components.FieldCentricDriveAbsolute;
-import org.firstinspires.ftc.teamcode.kronbot.utils.components.RobotCentricDrive;
-import org.firstinspires.ftc.teamcode.kronbot.utils.Constants;
-import org.firstinspires.ftc.teamcode.kronbot.utils.wrappers.Button;
-
-/**
- * A test TeleOP program for the driving period of the game.
- *
- * @version 1.0
- */
-@TeleOp(name = "Idle Driving", group = Constants.MAIN_GROUP)
-public class IdleOp extends LinearOpMode {
- private final KronBot robot = new KronBot();
-
- RobotCentricDrive robotCentricDrive;
- FieldCentricDrive fieldCentricDrive;
-
- Gamepad drivingGamepad;
-
- @Override
- public void runOpMode() throws InterruptedException {
- robot.initSimpleDriving(hardwareMap);
-
- drivingGamepad = gamepad1;
-
-
- while (!isStopRequested() && !opModeIsActive()) {
- telemetry.addLine("Initialization Ready");
- telemetry.update();
- }
-
- if (isStopRequested()) return;
-
- while (opModeIsActive() && !isStopRequested()) {
- telemetry.addLine("Plangesc");
-
- telemetry.update();
- }
- }
-}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingNewOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingNewOp.java
index 3053a80..80feead 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingNewOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingNewOp.java
@@ -25,7 +25,7 @@
* The main TeleOP program for the driving period of the game.
* @version 1.0
*/
-@TeleOp(name = "New Main Driving", group = Constants.MAIN_GROUP)
+//@TeleOp(name = "New Main Driving", group = Constants.MAIN_GROUP)
public class MainDrivingNewOp extends LinearOpMode {
private final KronBot robot = new KronBot();
private RobotCentricDrive robotCentricDrive;
@@ -47,7 +47,7 @@ public class MainDrivingNewOp extends LinearOpMode {
public void runOpMode() throws InterruptedException {
// INIT objects
- robot.initTeleop(hardwareMap);
+ robot.initHardware(hardwareMap);
dashboard = FtcDashboard.getInstance();
drivingGamepad = gamepad1;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java
index b451745..6f602e2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java
@@ -26,6 +26,7 @@
import org.firstinspires.ftc.teamcode.kronbot.utils.components.RobotCentricDrive;
import org.firstinspires.ftc.teamcode.kronbot.utils.Constants;
import org.firstinspires.ftc.teamcode.kronbot.utils.components.TurretAligner;
+import org.firstinspires.ftc.teamcode.kronbot.utils.misc.LpsCounter;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
/**
@@ -42,21 +43,23 @@ public class MainDrivingOp extends OpMode {
private TurretAligner turretAligner;
- private RobotCentricDrive robotCentricDrive;
- private FieldCentricDrive fieldCentricDrive;
private FtcDashboard dashboard;
private boolean autoAimEnabled = false;
ElapsedTime turretTimer = new ElapsedTime();
+ LpsCounter lpsCounter;
+
boolean rumbled = false;
@Override
public void init(){
+ lpsCounter = new LpsCounter();
+ lpsCounter.getLoopTime();
+ robot.initFollower(hardwareMap);
robot.init(hardwareMap);
- robot.initTeleop(hardwareMap);
- robot.loader.reversed = true;
+
dashboard = FtcDashboard.getInstance();
robot.webcam.init(hardwareMap, telemetry);
@@ -71,27 +74,39 @@ public void init(){
drivingGP = new Controls(gamepad1);
utilityGP = new Controls(gamepad2);
+
+ try {
+ robot.follower.getPoseTracker().resetIMU();
+ } catch (InterruptedException e) {
+ throw new RuntimeException(e);
+ }
}
@Override
public void init_loop(){
+ lpsCounter.getLoopTime();
+
telemetry.addLine("Initialization Ready");
telemetry.update();
}
@Override
public void start(){
- robotCentricDrive = new RobotCentricDrive(robot, gamepad1);
- fieldCentricDrive = new FieldCentricDrive(robot, gamepad1);
+
+ robot.follower.startTeleopDrive();
+
}
@Override
public void loop(){
+ // Update Loops/s delta
+ lpsCounter.getLoopTime();
+
//Update controller inputs
drivingGP.update();
utilityGP.update();
-
+ robot.follower.update();
//Intake
robot.intake.speed = utilityGP.rightStick.y;
@@ -177,27 +192,32 @@ else if(drivingGP.dpadDown.pressed())
//Shoot Close/Far
if (drivingGP.triangle.justPressed()) {
+ robot.turret.autoAimEnabled = false;
robot.shoot.activateRange(1, gamepad1);
}
if(drivingGP.square.justPressed()) {
+ robot.turret.autoAimEnabled = false;
robot.shoot.activateRange(2, gamepad1);
}
if (drivingGP.cross.justPressed()) {
+ robot.turret.autoAimEnabled = false;
robot.shoot.activateRange(3, gamepad1);
}
if(drivingGP.circle.justPressed()) {
+ robot.turret.autoAimEnabled = false;
robot.shoot.activateRange(4, gamepad1);
}
if( robot.outtake.on &&
- robot.leftOuttake.getVelocity() >= robot.outtake.velocity - 30 &&
- robot.leftOuttake.getVelocity() <= robot.outtake.velocity + 90 )
+ robot.leftOuttake.getVelocity() >= robot.outtake.velocity - 30 &&
+ robot.leftOuttake.getVelocity() <= robot.outtake.velocity + 90 )
{
gamepad1.rumble(1, 0, 150);
rumbled = true;
}
if(!autoAimEnabled && drivingGP.leftBumper.justPressed()) {
+ robot.turret.autoAimEnabled = true;
if(robot.outtake.on) {
robot.shoot.deactivate();
gamepad1.rumble(1, 1, 100);
@@ -206,30 +226,12 @@ else if(drivingGP.dpadDown.pressed())
}
//Update robot systems status
- movement();
+ robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true);
robot.updateAllSystems();
_telemetry();
//robot.webcam.update();
}
- private boolean reverseMovement=false, drivingMode=false;//False for robot centric, true for field centric
- private void movement(){
- if(drivingGP.rightStick.button.shortPressed())
- reverseMovement = !reverseMovement;
-
-// if(drivingGP.circle.longPressed())
-// drivingMode = !drivingMode;
-
- robotCentricDrive.setReverse(reverseMovement);
- if (!drivingMode) {
- robotCentricDrive.run();
- robotCentricDrive.telemetry(telemetry);
- } else {
- fieldCentricDrive.run();
- fieldCentricDrive.telemetry(telemetry);
- }
-
- }
@Override
public void stop(){
@@ -237,6 +239,10 @@ public void stop(){
}
public void _telemetry(){
+ telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta);
+ telemetry.addData("x", robot.follower.getPose().getX());
+ telemetry.addData("y", robot.follower.getPose().getY());
+ telemetry.addData("heading", robot.follower.getPose().getHeading());
telemetry.addData("Heading", robot.follower.getHeading());
telemetry.addData("shooter motor vel:", robot.leftOuttake.getVelocity());
telemetry.addData("angle servo pos:", robot.turretServo.getPosition());
@@ -244,7 +250,8 @@ public void _telemetry(){
robot.intake.telemetry(telemetry);
robot.loader.telemetry(telemetry);
robot.outtake.telemetry(telemetry);
-// robot.turret.telemetry(telemetry);
+ robot.heading.telemetry(telemetry);
+ robot.turret.telemetry(telemetry);
drivingGP.telemetry(telemetry);
telemetry.update();
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/NewFieldCentricExOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/NewFieldCentricExOp.java
index 12fd2f6..c673edd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/NewFieldCentricExOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/NewFieldCentricExOp.java
@@ -23,7 +23,7 @@ public class NewFieldCentricExOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
- robot.initSimpleDriving(hardwareMap);
+ robot.initHardware(hardwareMap);
drivingGamepad = gamepad1;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java
index 453cef2..f8c1900 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/OuttakeShootingTesting.java
@@ -37,7 +37,6 @@ public class OuttakeShootingTesting extends OpMode {
@Override
public void init(){
robot.init(hardwareMap);
- robot.loader.reversed = true;
drivingGP = new Controls(gamepad1);
utilityGP = new Controls(gamepad2);
@@ -75,8 +74,6 @@ public void loop(){
else
robot.intake.reversed = false;
- telemetry.addData("Ultrasonic Distance", "%.0f", robot.rangeSensor.cmUltrasonic());
-
anglePos += -drivingGP.rightStick.y * 0.01;
anglePos = Math.max(Math.min(anglePos, ANGLE_SERVO_MAX), ANGLE_SERVO_MIN);
shooterVel += drivingGP.leftStick.y * 5;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/PinpointOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/PinpointOp.java
index bee969b..d920bbc 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/PinpointOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/PinpointOp.java
@@ -1,10 +1,7 @@
package org.firstinspires.ftc.teamcode.kronbot.manual;
-import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.coordinates;
-import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.TestPoseStart;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -12,7 +9,6 @@
import org.firstinspires.ftc.teamcode.kronbot.Robot;
import org.firstinspires.ftc.teamcode.kronbot.utils.Constants;
import org.firstinspires.ftc.teamcode.kronbot.utils.Controls;
-import org.firstinspires.ftc.teamcode.kronbot.utils.PoseStorage;
import org.firstinspires.ftc.teamcode.kronbot.utils.components.FieldCentricDrive;
import org.firstinspires.ftc.teamcode.kronbot.utils.components.RobotCentricDrive;
@@ -41,7 +37,7 @@ public void init() {
utilityGP = new Controls(gamepad2);
// create pedro follower with pinpoint localizer
- robot.initAutonomy(hardwareMap);
+ robot.initHardware(hardwareMap);
FtcDashboard dashboard = FtcDashboard.getInstance();
telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/SimpleDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/SimpleDrivingOp.java
index 5261db8..b00f0f1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/SimpleDrivingOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/SimpleDrivingOp.java
@@ -28,7 +28,7 @@ public class SimpleDrivingOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
- robot.initSimpleDriving(hardwareMap);
+ robot.initHardware(hardwareMap);
drivingGamepad = gamepad1;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/TurretDataOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/TurretDataOp.java
index 0d5840a..a707e77 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/TurretDataOp.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/TurretDataOp.java
@@ -42,7 +42,7 @@ public class TurretDataOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
try {
- robot.initTeleop(hardwareMap);
+ robot.initHardware(hardwareMap);
dashboard = FtcDashboard.getInstance();
telemetry.update();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java
index a46cc02..56f264c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java
@@ -20,6 +20,7 @@ public class Constants {
public final static String TEST_GROUP = "test";
public final static String MAIN_GROUP = "main";
+
public static double CONTROLLER_DEADZONE = 0.15;
public static int BUTTON_LONG_PRESS_TIME = 750;
@@ -61,15 +62,15 @@ public class Constants {
public static double maxVelocity = 1500;
public static double RANGE_1_ANGLE = 0;
- public static double RANGE_1_VELOCITY = 950;
+ public static double RANGE_1_VELOCITY = 1150;
public static double RANGE_1_KS = 0.15;
- public static double RANGE_2_ANGLE = 0.5;
- public static double RANGE_2_VELOCITY = 1080;
+ public static double RANGE_2_ANGLE = 0.3;
+ public static double RANGE_2_VELOCITY = 1280;
public static double RANGE_2_KS = 0.2;
public static double RANGE_3_ANGLE = 0.72;
- public static double RANGE_3_VELOCITY = 1250;
+ public static double RANGE_3_VELOCITY = 1350;
public static double RANGE_3_KS = 0.3;
public static double RANGE_4_ANGLE = 0.72;
@@ -100,6 +101,7 @@ public class Constants {
public static double AIM_KI = 0.0;
public static double AIM_KD = 0.003;
public static double ANGLE_TOLERANCE = 2.0;
+ public static double DELTA_THRESHOLD = 0.01;
public static double MAX_ROTATION_POWER = 0.5;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java
index 7f9f844..00ebddd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/PoseStorage.java
@@ -1,46 +1,61 @@
package org.firstinspires.ftc.teamcode.kronbot.utils;
+import android.os.Environment;
+
import com.pedropathing.geometry.Pose;
import com.qualcomm.robotcore.util.RobotLog;
-import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
-
import java.io.*;
public class PoseStorage {
private static final String FILE_NAME = "lastPose.txt";
- //save the pose
+ // SAVE pose to external storage
public static void savePose(Pose pose) {
- File file = AppUtil.getInstance().getSettingsFile(FILE_NAME);
- try (PrintWriter writer = new PrintWriter(new FileWriter(file))) {
+ String path = Environment.getExternalStorageDirectory().getPath()
+ + "/" + FILE_NAME;
+
+ try (PrintWriter writer = new PrintWriter(new FileWriter(path))) {
+
writer.println(pose.getX());
writer.println(pose.getY());
writer.println(pose.getHeading());
+
+ RobotLog.ii("PoseStorage", "Pose saved to " + path);
+
} catch (IOException e) {
RobotLog.ee("PoseStorage", "Failed to save pose", e);
}
}
- //load the pose
+ // LOAD pose from external storage
public static Pose loadPose() {
- File file = AppUtil.getInstance().getSettingsFile(FILE_NAME);
+
+ String path = Environment.getExternalStorageDirectory().getPath()
+ + "/" + FILE_NAME;
+
+ File file = new File(path);
if (!file.exists()) {
+ RobotLog.ww("PoseStorage", "Pose file not found");
return new Pose(0, 0, 0);
}
try (BufferedReader reader = new BufferedReader(new FileReader(file))) {
+
double x = Double.parseDouble(reader.readLine());
double y = Double.parseDouble(reader.readLine());
double heading = Double.parseDouble(reader.readLine());
+ RobotLog.ii("PoseStorage", "Pose loaded from " + path);
+
return new Pose(x, y, heading);
+
} catch (Exception e) {
RobotLog.ee("PoseStorage", "Failed to load pose", e);
return new Pose(0, 0, 0);
}
}
-}
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/OuttakeStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/OuttakeStateMachine.java
new file mode 100644
index 0000000..a6388ab
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/OuttakeStateMachine.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.kronbot.utils.components;
+
+import org.firstinspires.ftc.teamcode.kronbot.Robot;
+
+public class OuttakeStateMachine {
+ private Robot robot;
+ public OuttakeStateMachine(Robot robot) {
+ this.robot = robot;
+ }
+
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/RobotCentricDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/RobotCentricDrive.java
index 23f3d21..d2cfaaf 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/RobotCentricDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/RobotCentricDrive.java
@@ -44,9 +44,9 @@ public void run() {
double leftRearPower = (y - x + r) / normalizer;
double rightRearPower = (y + x - r) / normalizer;
- robot.motors.leftFront.setPower(leftFrontPower);
robot.motors.rightFront.setPower(rightFrontPower);
robot.motors.leftRear.setPower(leftRearPower);
+ robot.motors.leftFront.setPower(leftFrontPower);
robot.motors.rightRear.setPower(rightRearPower);
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/TurretAligner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/TurretAligner.java
index d573989..d7059b7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/TurretAligner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/TurretAligner.java
@@ -5,8 +5,7 @@
public class TurretAligner {
private final Robot robot;
- private double targetX = 0;
- private double targetY = 0;
+ private Pose target;
// Simple P-gain if you want to smooth the movement
// (1.0 means it snaps instantly to the target)
@@ -17,22 +16,22 @@ public TurretAligner(Robot robot) {
}
public void setTarget(double x, double y) {
- this.targetX = x;
- this.targetY = y;
+ target = new Pose(x, y);
}
public void update() {
Pose currentPose = robot.follower.getPose();
- // 1. Calculate the difference
- double dx = targetX - currentPose.getX();
- double dy = targetY - currentPose.getY();
+ // Angle from robot to the target
+ double r = Math.atan2(target.getY() - currentPose.getY(), target.getX() - currentPose.getX());
+
+ double turret = r - currentPose.getHeading();
+ if(Math.abs(turret) > Math.PI) {
+ turret += 2 * Math.PI;
+ }
+
- // 2. Calculate the global angle to the target (radians)
- // atan2 takes (y, x)
- double globalTargetAngle = Math.atan2(dy, dx);
- robot.turret.angle = globalTargetAngle;
}
/**
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/misc/LpsCounter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/misc/LpsCounter.java
new file mode 100644
index 0000000..9f003c0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/misc/LpsCounter.java
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.kronbot.utils.misc;
+
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class LpsCounter {
+ ElapsedTime timer;
+ double lastTime;
+
+ /** The time between the last two calls */
+ public double delta;
+
+ /**
+ * Get the time between the last call of the function and this call.
+ * Call once at initialization and once per loop.
+ * This function updates delta.
+ * @return The time between calls, in seconds.
+ */
+ public double getLoopTime() {
+ if(timer == null) {
+ timer = new ElapsedTime(ElapsedTime.Resolution.SECONDS);
+ lastTime = 0;
+ return 1;
+ }
+
+ delta = timer.time() - lastTime;
+ lastTime = timer.time();
+
+ return delta;
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java
index 3eeb430..66f9f62 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/tests/TestAuto.java
@@ -24,7 +24,7 @@ public class TestAuto extends LinearOpMode {
KronBot robot = new KronBot();
@Override
public void runOpMode() throws InterruptedException {
- robot.initAutonomy(hardwareMap);
+ robot.initHardware(hardwareMap);
Follower follower = Constants.createFollower(hardwareMap);
telemetry.addLine(follower == null ? "Follower is NULL!" : "Follower created!");
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
index babfaa6..eb697a3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
@@ -53,7 +53,7 @@ public class Constants {
.leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
.rightFrontMotorDirection(DcMotorSimple.Direction.REVERSE)
.rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
-
+ .useBrakeModeInTeleOp(true)
.xVelocity(93.675629)
.yVelocity(77.158601);
@@ -84,7 +84,7 @@ public class Constants {
.distanceUnit(DistanceUnit.INCH)
.hardwareMapName("pinpoint")
.encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
- .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED)
+ .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD)
.strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD)
;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
index aa40842..bd72531 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
@@ -126,7 +126,9 @@ public static void stopRobot() {
*/
class LocalizationTest extends OpMode {
@Override
- public void init() {}
+ public void init() {
+
+ }
/** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */
@Override
diff --git a/gradle/gradle-daemon-jvm.properties b/gradle/gradle-daemon-jvm.properties
new file mode 100644
index 0000000..aeb375c
--- /dev/null
+++ b/gradle/gradle-daemon-jvm.properties
@@ -0,0 +1,13 @@
+#This file is generated by updateDaemonJvm
+toolchainUrl.FREE_BSD.AARCH64=https\://api.foojay.io/disco/v3.0/ids/29ee363f71d060405f729a8f1b7f7aef/redirect
+toolchainUrl.FREE_BSD.X86_64=https\://api.foojay.io/disco/v3.0/ids/67a0fee3c4236b6397dcbe8575ca2011/redirect
+toolchainUrl.LINUX.AARCH64=https\://api.foojay.io/disco/v3.0/ids/536afcd1dff540251f85e5d2c80458cf/redirect
+toolchainUrl.LINUX.X86_64=https\://api.foojay.io/disco/v3.0/ids/67a0fee3c4236b6397dcbe8575ca2011/redirect
+toolchainUrl.MAC_OS.AARCH64=https\://api.foojay.io/disco/v3.0/ids/0b98aec810298c2c1d7fdac5dac37910/redirect
+toolchainUrl.MAC_OS.X86_64=https\://api.foojay.io/disco/v3.0/ids/9c55677aff3966382f3d853c0959bfb2/redirect
+toolchainUrl.UNIX.AARCH64=https\://api.foojay.io/disco/v3.0/ids/29ee363f71d060405f729a8f1b7f7aef/redirect
+toolchainUrl.UNIX.X86_64=https\://api.foojay.io/disco/v3.0/ids/67a0fee3c4236b6397dcbe8575ca2011/redirect
+toolchainUrl.WINDOWS.AARCH64=https\://api.foojay.io/disco/v3.0/ids/fddf6293b7b991bd01fa68e5b2eab7ec/redirect
+toolchainUrl.WINDOWS.X86_64=https\://api.foojay.io/disco/v3.0/ids/ac151d55def6b6a9a159dc4cb4642851/redirect
+toolchainVendor=JETBRAINS
+toolchainVersion=21
diff --git a/settings.gradle b/settings.gradle
index bd0f4b2..afcfdcd 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -1,5 +1,7 @@
plugins {
id 'org.gradle.toolchains.foojay-resolver-convention' version '1.0.0'
-}
+}//plugins {
+// id 'org.gradle.toolchains.foojay-resolver-convention' version '0.8.0'
+//}
include ':FtcRobotController'
include ':TeamCode'