Skip to content

Commit

Permalink
Merge pull request #6 from Pioneer-Robotics/specimen_auto
Browse files Browse the repository at this point in the history 
Merge Post-Tournament 2 Code.
  • Loading branch information
@Braedeng0
Braedeng0 authored Jan 27, 2025
2 parents d578045 + 11c4fbd commit 77e5c1d
Show file tree
Hide file tree
Showing 26 changed files with 986 additions and 289 deletions.
12 changes: 10 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,12 @@
import org.firstinspires.ftc.teamcode.Hardware.BotIMU;
import org.firstinspires.ftc.teamcode.Hardware.ColorSensor;
import org.firstinspires.ftc.teamcode.Hardware.CurrentDetection;
import org.firstinspires.ftc.teamcode.Hardware.DiffyClaw;
import org.firstinspires.ftc.teamcode.Hardware.Intake;
import org.firstinspires.ftc.teamcode.Hardware.IntakeClaw;
import org.firstinspires.ftc.teamcode.Hardware.LEDController;
import org.firstinspires.ftc.teamcode.Hardware.MecanumBase;
import org.firstinspires.ftc.teamcode.Hardware.OCGBox;
import org.firstinspires.ftc.teamcode.Hardware.SlideArm;
import org.firstinspires.ftc.teamcode.Hardware.SpecimenArm;
import org.firstinspires.ftc.teamcode.Hardware.VoltageHandler;
Expand Down Expand Up @@ -47,6 +50,8 @@ public class Bot {
public static CurrentUtils currentThreads;
public static FtcDashboard dashboard;
public static Telemetry dashboardTelemetry;
public static IntakeClaw intakeClaw;
public static OCGBox ocgBox;

/**
* Constructor for Bot.
Expand All @@ -59,8 +64,8 @@ public static void init(@NonNull LinearOpMode opMode, double startX, double star

// Drive base and self driving
Bot.pinpoint = new Pinpoint(startX, startY);
Bot.optical_odom = new SparkfunOTOS();
Bot.deadwheel_odom = new TwoWheelOdometry(Config.specimenStartX, Config.specimenStartY);
// Bot.optical_odom = new SparkfunOTOS();
// Bot.deadwheel_odom = new TwoWheelOdometry(Config.specimenStartX, Config.specimenStartY);
Bot.mecanumBase = new MecanumBase();
Bot.pidDrive = new PIDDrive();
Bot.purePursuit = new PurePursuit(Config.drivePID[0], Config.drivePID[1], Config.drivePID[2]);
Expand All @@ -71,6 +76,9 @@ public static void init(@NonNull LinearOpMode opMode, double startX, double star

// Servos
Bot.intake = new Intake();
Bot.intakeClaw = new IntakeClaw();
Bot.ocgBox = new OCGBox();


// Other
Bot.frontTouchSensor = opMode.hardwareMap.get(AnalogInput.class, Config.touchSensor);
Expand Down
134 changes: 90 additions & 44 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Config.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,11 @@
// Annotation to allow quick changes to config from FTC Dashboard
@com.acmerobotics.dashboard.config.Config
public class Config {
// ---- Constants ----

/* --------------------
---- Constants ----
-------------------- */

// Odometer constants
public static final double wheelDiameter = 4.8;
public static final double ticsPerRev = 2000;
Expand All @@ -16,7 +20,7 @@ public class Config {
public static final double forwardOffset = Math.sqrt((15.5 * 15.5) - 0.4); // In CM

// Encoder constants
public static final double maxDriveTicksPerSecond = 2700; // ~300 RPM
public static final double maxDriveTicksPerSecond = 2500; // ~300 RPM
public static final double maxSlideTicksPerSecond = 2700;
public static final double maxSpecTicksPerSecond = 2700;

Expand All @@ -26,26 +30,33 @@ public class Config {

// PID constants
public static double[] drivePID = {0.08, 0.0001, 0.25}; // kP, kI, kD
public static double[] turnPID = {3, 0.0001, 0.15}; // kP, kI, kD
public static double[] turnPID = {2.5, 0.00001, 0.1}; // kP, kI, kD
public static double driveSpeed = 0.4;

// Feedforward constants
public static double fLF = 0.0015;
public static double fRF = 0.0015;
public static double fLB = 0.0015;
public static double fRB = 0.0015;

// Tolerances
// How close the robot needs to be to the target position to stop (in cm)
public static double driveTolerance = 1.5;
// How close the robot needs to be to the target angle to stop (in radians)
public static double turnTolerance = 0.05;
public static double turnTolerance = 0.25;
public static final double specimenArmTolerance = 5; // Motor ticks

public static double lookAhead = 15; // Pure pursuit lookahead

// Used to move "virtual robot" ahead of actual robot in pure pursuit
public static double overshootDistance(double velocity) {
// Polynomial fit for overshoot distance
return ((0.000714 * velocity * velocity) + (0.145 * velocity) + 0.216) * 1.1;
return ((0.000714 * velocity * velocity) + (0.145 * velocity) + 0.216); // *1.075
}

// Used to gradually accelerate
// Multiplier starts at 0.1 and increments by acceleration each loop up to 1
// Only used in PID Drive (deprecated)
public static final double acceleration = 0.04;

// Color sensor
Expand All @@ -54,7 +65,9 @@ public static double overshootDistance(double velocity) {
public static final int[] colorYellow = {410, 460, 110}; // Yellow sample color (rgb)
public static final int alphaTolerance = 100; // If alpha is above this value, a sample is detected

// ---- Hardware Map ----
/* --------------------
--- Hardware Map ---
-------------------- */
// Odometers
public static final String odoLeft = "odoLeft";
public static final String odoCenter = "odoCenter";
Expand All @@ -68,7 +81,8 @@ public static double overshootDistance(double velocity) {

// Motor names
public static final String specimenArmMotor = "specimenMotor";
public static final String slideMotor = "slideMotor";
public static final String slideMotor1 = "slideMotor1";
public static final String slideMotor2 = "slideMotor2";

// Servo names
public static final String clawServo = "clawServo";
Expand All @@ -79,56 +93,77 @@ public static double overshootDistance(double velocity) {
public static final String intakeClaw = "intakeClaw";
public static final String intakeWrist = "intakeWrist";
public static final String ocgBox = "ocgBox";
public static final String diffyServo1 = "diffyServo1";
public static final String diffyServo2 = "diffyServo2";
public static final String ocgPitchServo = "pitchServo";
public static final String ocgRollServo = "rollServo";
public static final String intakeYawServo = "intakeYawServo";
public static final String intakeRollServo = "intakeRollServo";
public static final String intakeClawServo = "intakeClawServo";

// Other names
public static final String touchSensor = "touch";
public static final String colorSensor = "colorSensor";
public static final String led = "led";
public static final String imu = "expansionIMU";

// ---- Servo Positions ----
/* --------------------
- Servo Positions -
-------------------- */
// Specimen Arm Claw
public static final double clawOpen = 0.35;
public static final double clawClose = 0.95;
// Intake Claw
public static final double intakeClawOpen = 0.4;
public static final double intakeClawClose = 0;
public static final double clawOpen = 0.3;
public static final double clawClose = 0.5;
// Misumi Drive
//Right isnt used rn
public static final double misumiDriveLOpen = 0.6;
public static final double misumiDriveLMid = 0.4;
public static final double misumiDriveLClose = 0.375;

public static final double misumiDriveROpen = 0.625;
public static final double misumiDriveRMid = 0.4;
public static final double misumiDriveRClose = 0.35;
public static final double misumiDriveLOpen = 0.475;
public static final double misumiDriveLMid = 0.3;
public static final double misumiDriveLClose = 0.2;
// public static double misumiDriveROpen = 0.175;
// public static double misumiDriveRMid = 0.4;
// public static double misumiDriveRClose = 0.525;
// Misumi Wrist
//Left isnt used rn
public static final double misumiWristLOpen = 0.225; // Up to OCG box
public static final double misumiWristLMid = 0.5; // 0.525 touching bar
public static final double misumiWristLInit = 0.475;
public static final double misumiWristLClose = 0.565;

public static final double misumiWristROpen = 0.7; //Up to OCG box //Was 0.675
public static final double misumiWristRMid = 0.5; // 0.325 touching bar //Was 0.35
public static final double misumiWristRInit = 0.6;
public static final double misumiWristRClose = 0.35; //Was 0.285
public static final double misumiWristLDown = 0.85;
public static final double misumiWristLMid = 0.75;
public static final double misumiWristLUp = 0.5; // Up to OCG box
public static final double misumiWristRDown = 0.15;
public static final double misumiWristRMid = 0.24;
public static final double misumiWristRUp = 0.4;
// Intake Wrist
public static final double intakeWristClose = 0;
public static final double intakeWristOpen = 1;
// Intake Claw
public static final double intakeYawLeft = 0.05;
public static final double intakeYawRight = 0.58;
public static final double intakeYawMid = 0.85; //Temporary
public static final double intakeRollUp = 0.43;
public static final double intakeRollDown = 0.96;
public static final double intakeClawOpen = 1;
public static final double intakeClawClose = 0.5;
// OCG Box
public static final double ocgBoxDropRight = 0.1;
public static final double ocgBoxHold = 0.375;
public static final double ocgBoxDrop = 0.8;

// ---- Motor Positions ----
public static final double ocgBoxPitchUp = 0.705;
public static final double ocgBoxPitchDown = 0.275;
public static final double ocgBoxRollUp = 0.867;
public static final double ocgBoxRollDown = 0.635;

/* --------------------
- Motor Positions -
-------------------- */
// Specimen Arm
public static final double defaultSpecimenArmSpeed = 0.5;
public static int specimenArmPostHang = 1350;
public static int specimenArmPrepHang = 900;
public static int specimenArmCollect = 1980;
public static final int specimenArmPrepHangUp = 1050;
public static final int specimenArmPostHangUp = 750;
public static final double defaultSpecimenArmSpeed = 0.4;
public static int specimenArmPostHang = -1250;
public static int specimenArmPrepHang = -800;
// public static int specimenArmPostHang = 800;
// public static int specimenArmPrepHang = 1250;
// public static final int specimenArmPrepHangUp = 1000;
// public static final int specimenArmPostHangUp = 650;
public static final int specimenArmPrepHangUp = -1000;
public static final int specimenArmPostHangUp = -650;
public static final int specimenArmCollect1 = 2010;
public static final int specimenArmCollect2 = 0;
// public static final int specimenArmCollect1 = -2010;
// public static final int specimenArmCollect2 = -0;
// Linear Slide
public static final double defaultSlideSpeed = 0.25;
public static final int minSlideHeight = 15;
Expand All @@ -137,22 +172,33 @@ public static double overshootDistance(double velocity) {
public static final int slideLowBasket = 2500;
public static final int slideHighBasket = 4300;

// ---- Coordinates of note and auto config ----
/* -------------------------
- Coordinates of Note -
- and Auto Config -
------------------------- */

// Specimen hang offset (space between hangs)
public static final double hangOffset = 6.5;

// Coordinates of first specimen hang
// Coordinates of default specimen hang
public static final double specHangX = 195;
public static final double specHangY = 96.25;
public static final double specHangY = 94.95;

// Coordinates of observation zone park
public static final double parkX = 300;
public static final double parkY = 30;

// Robot starting position for specimen auto
// Robot starting position for autos
public static final double specimenStartX = 198;
public static final double specimenStartY = 20.5;
public static final double specimenStartY = 21.5;
public static final double sampleStartX = 85;
public static final double sampleStartY = 20.5;

// Hermite Points of Interest
public static final double[] pickSample1 = {58, 65};
public static final double[] pickSample2 = {25, 65};
public static final double[] pickSample3 = {25, 65};
public static final double[] submersablePickup = {120, 160};

// ---- Misc ----
public static final double defaultMaxCurrent = 8000;
Expand Down
85 changes: 81 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware/CurrentDetection.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,26 @@
public class CurrentDetection {
private volatile double current;
private final double maxCurrent;
private final boolean isSpecimenArm;
private final boolean isSlideArm;
private final DcMotorEx motor;
Timer timer;
TimerTask task;
public CurrentDetection(DcMotorEx motor, double maxCurrent) {

public CurrentDetection(DcMotorEx motor, double maxCurrent, boolean isSpecimenArm, boolean isSlideArm) {
this.motor = motor;
this.isSpecimenArm = isSpecimenArm;
this.isSlideArm = isSlideArm;
this.current = Float.POSITIVE_INFINITY;
this.maxCurrent = maxCurrent;
}

public CurrentDetection(DcMotorEx motor, double maxCurrent) {
this(motor, maxCurrent, false, false);
}

public CurrentDetection(DcMotorEx motor) {
this(motor, Config.defaultMaxCurrent);
this(motor, Config.defaultMaxCurrent, false, false);
}

public void checkCurrent() {
Expand All @@ -37,6 +46,63 @@ public void checkCurrent() {
}
}

public void checkSpecimenCurrent() {
Bot.dashboardTelemetry.addData("Specimen Arm Pos", motor.getCurrentPosition());
Bot.dashboardTelemetry.addData("Diff to pos1", Math.abs(Config.specimenArmCollect1 - motor.getCurrentPosition()));
Bot.dashboardTelemetry.addData("Diff to pos2", Math.abs(Config.specimenArmCollect2 - motor.getCurrentPosition()));

Bot.dashboardTelemetry.update();
current = motor.getCurrent(CurrentUnit.MILLIAMPS);
// Virtual touch sensor
if (current > maxCurrent && ((Math.abs(Config.specimenArmCollect1 - motor.getCurrentPosition()) < 300) ||
(Math.abs(Config.specimenArmCollect2 - motor.getCurrentPosition()) < 300))) {
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setPower(0);
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
}
if (current > 8000) {
Bot.opMode.telemetry.addLine("MOTOR REACHED MAX CURRENT");
Bot.opMode.gamepad1.rumble(500);
Bot.opMode.gamepad2.rumble(500);
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setPower(0);
}
}

public void checkSpecimenCurrentHome(){
Bot.dashboardTelemetry.addData("Specimen Arm Pos", motor.getCurrentPosition());
Bot.dashboardTelemetry.addData("Diff to pos1", Math.abs(Config.specimenArmCollect1 - motor.getCurrentPosition()));
Bot.dashboardTelemetry.addData("Diff to pos2", Math.abs(Config.specimenArmCollect2 - motor.getCurrentPosition()));

Bot.dashboardTelemetry.update();
current = motor.getCurrent(CurrentUnit.MILLIAMPS);
// Virtual touch sensor
if (current > maxCurrent) {
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setPower(0);
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
}
if (current > 8000) {
Bot.opMode.telemetry.addLine("MOTOR REACHED MAX CURRENT");
Bot.opMode.gamepad1.rumble(500);
Bot.opMode.gamepad2.rumble(500);
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setPower(0);
}
}

public void checkSlideCurrent() {
current = motor.getCurrent(CurrentUnit.MILLIAMPS);
if (current > maxCurrent) {
Bot.opMode.telemetry.addLine("MOTOR REACHED MAX CURRENT");
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setPower(0);
Bot.opMode.gamepad1.rumble(500);
Bot.opMode.gamepad2.rumble(500);
motor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
}
}

public double getCurrent() {
return current;
}
Expand All @@ -50,9 +116,20 @@ public void start() {
task = new TimerTask() {
@Override
public void run() {
checkCurrent();
if (isSlideArm) {
checkSlideCurrent();
} if (isSpecimenArm) {
checkSpecimenCurrent();
} else {
checkCurrent();
}
Bot.opMode.telemetry.addLine("Checked");
Bot.opMode.telemetry.update();
if(Bot.opMode.isStopRequested()) {
stop();
}
}
};
timer.schedule(task, 0, 500);
timer.schedule(task, 100, 250);
}
}
Loading

0 comments on commit 77e5c1d

Please sign in to comment.