KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-03 15:55:35 -06:00
parent 07297c60f1
commit 4b998766a1
10 changed files with 307 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
View File

@@ -178,8 +178,8 @@ public class Blue_V2 extends LinearOpMode {
TELE.update(); TELE.update();
if (gpp || pgp || ppg){ if (gpp || pgp || ppg){
robot.turr1.setPosition(turret_blue); robot.turr1.setPower(turret_blue);
robot.turr2.setPosition(1 - turret_blue); robot.turr2.setPower(1 - turret_blue);
return false; return false;
} else { } else {
return true; return true;
@@ -203,8 +203,8 @@ public class Blue_V2 extends LinearOpMode {
position = spindexer_intakePos1 - 0.02; position = spindexer_intakePos1 - 0.02;
} }
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
if (ticker == 0) { if (ticker == 0) {
stamp = getRuntime(); stamp = getRuntime();
@@ -264,8 +264,8 @@ public class Blue_V2 extends LinearOpMode {
velo = flywheel.getVelo(); velo = flywheel.getVelo();
robot.shooter1.setPower(powPID); robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID); robot.shooter2.setPower(powPID);
robot.spin1.setPosition(spindexer); robot.spin1.setPower(spindexer);
robot.spin2.setPosition(1-spindexer); robot.spin2.setPower(1-spindexer);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addLine("spindex"); TELE.addLine("spindex");
TELE.update(); TELE.update();
@@ -380,8 +380,8 @@ public class Blue_V2 extends LinearOpMode {
} else { } else {
position = spindexer_intakePos1 - 0.02; position = spindexer_intakePos1 - 0.02;
} }
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addLine("Intaking"); TELE.addLine("Intaking");
@@ -418,8 +418,8 @@ public class Blue_V2 extends LinearOpMode {
} else { } else {
position = spindexer_intakePos1 - 0.02; position = spindexer_intakePos1 - 0.02;
} }
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM); double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -541,13 +541,13 @@ public class Blue_V2 extends LinearOpMode {
robot.hood.setPosition(hoodAuto); robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectBlue); robot.turr1.setPower(turret_detectBlue);
robot.turr2.setPosition(1 - turret_detectBlue); robot.turr2.setPower(1 - turret_detectBlue);
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexer_intakePos1); robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1); robot.spin2.setPower(1 - spindexer_intakePos1);
aprilTag.update(); aprilTag.update();
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
View File

@@ -171,8 +171,8 @@ public class Red_V2 extends LinearOpMode {
TELE.update(); TELE.update();
if (gpp || pgp || ppg){ if (gpp || pgp || ppg){
robot.turr1.setPosition(turret_red); robot.turr1.setPower(turret_red);
robot.turr2.setPosition(1 - turret_red); robot.turr2.setPower(1 - turret_red);
return false; return false;
} else { } else {
return true; return true;
@@ -190,8 +190,8 @@ public class Red_V2 extends LinearOpMode {
velo = flywheel.getVelo(); velo = flywheel.getVelo();
robot.shooter1.setPower(powPID); robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID); robot.shooter2.setPower(powPID);
robot.spin1.setPosition(spindexer); robot.spin1.setPower(spindexer);
robot.spin2.setPosition(1-spindexer); robot.spin2.setPower(1-spindexer);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addLine("spindex"); TELE.addLine("spindex");
TELE.update(); TELE.update();
@@ -272,8 +272,8 @@ public class Red_V2 extends LinearOpMode {
} else { } else {
position = spindexer_intakePos1 - 0.02; position = spindexer_intakePos1 - 0.02;
} }
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);
TELE.addLine("Intaking"); TELE.addLine("Intaking");
@@ -331,8 +331,8 @@ public class Red_V2 extends LinearOpMode {
} else { } else {
position = spindexer_intakePos1 - 0.02; position = spindexer_intakePos1 - 0.02;
} }
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM); double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -454,13 +454,13 @@ public class Red_V2 extends LinearOpMode {
robot.hood.setPosition(hoodAuto); robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectRed); robot.turr1.setPower(turret_detectRed);
robot.turr2.setPosition(1 - turret_detectRed); robot.turr2.setPower(1 - turret_detectRed);
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexer_intakePos1); robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1); robot.spin2.setPower(1 - spindexer_intakePos1);
aprilTag.update(); aprilTag.update();
TELE.addData("Velocity", velo); TELE.addData("Velocity", velo);

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -1,6 +1,6 @@
package org.firstinspires.ftc.teamcode.teleop; package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*; import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
@@ -126,8 +126,8 @@ public class TeleopV2 extends LinearOpMode {
aprilTagWebcam.init(new Robot(hardwareMap), TELE); aprilTagWebcam.init(new Robot(hardwareMap), TELE);
robot.turr1.setPosition(0.4); robot.turr1.setPower(0.4);
robot.turr2.setPosition(1 - 0.4); robot.turr2.setPower(1 - 0.4);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
@@ -181,14 +181,14 @@ public class TeleopV2 extends LinearOpMode {
position = spindexer_intakePos1 - 0.015; position = spindexer_intakePos1 - 0.015;
} }
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
} else if (reject) { } else if (reject) {
robot.intake.setPower(-1); robot.intake.setPower(-1);
double position = spindexer_intakePos1; double position = spindexer_intakePos1;
robot.spin1.setPosition(position); robot.spin1.setPower(position);
robot.spin2.setPosition(1 - position); robot.spin2.setPower(1 - position);
} else { } else {
robot.intake.setPower(0); robot.intake.setPower(0);
} }
@@ -363,8 +363,8 @@ public class TeleopV2 extends LinearOpMode {
if (!overrideTurr) { if (!overrideTurr) {
robot.turr1.setPosition(pos); robot.turr1.setPower(pos);
robot.turr2.setPosition(1 - pos); robot.turr2.setPower(1 - pos);
} }
if (gamepad2.dpad_right) { if (gamepad2.dpad_right) {
@@ -445,12 +445,12 @@ public class TeleopV2 extends LinearOpMode {
reject = true; reject = true;
if (getRuntime() % 3 > 1.5) { if (getRuntime() % 3 > 1.5) {
robot.spin1.setPosition(0); robot.spin1.setPower(0);
robot.spin2.setPosition(1); robot.spin2.setPower(1);
} else { } else {
robot.spin1.setPosition(1); robot.spin1.setPower(1);
robot.spin2.setPosition(0); robot.spin2.setPower(0);
} }
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
@@ -475,9 +475,9 @@ public class TeleopV2 extends LinearOpMode {
overrideTurr = true; overrideTurr = true;
double bearing = d20.ftcPose.bearing; double bearing = d20.ftcPose.bearing;
double finalPos = robot.turr1.getPosition() - (bearing / 1300); double finalPos = robot.turr1.getPower() - (bearing / 1300);
robot.turr1.setPosition(finalPos); robot.turr1.setPower(finalPos);
robot.turr2.setPosition(1 - finalPos); robot.turr2.setPower(1 - finalPos);
TELE.addData("Bear", bearing); TELE.addData("Bear", bearing);
@@ -487,9 +487,9 @@ public class TeleopV2 extends LinearOpMode {
overrideTurr = true; overrideTurr = true;
double bearing = d24.ftcPose.bearing; double bearing = d24.ftcPose.bearing;
double finalPos = robot.turr1.getPosition() - (bearing / 1300); double finalPos = robot.turr1.getPower() - (bearing / 1300);
robot.turr1.setPosition(finalPos); robot.turr1.setPower(finalPos);
robot.turr2.setPosition(1 - finalPos); robot.turr2.setPower(1 - finalPos);
} }
@@ -543,8 +543,8 @@ public class TeleopV2 extends LinearOpMode {
} else { } else {
// Finished shooting all balls // Finished shooting all balls
robot.spin1.setPosition(spindexer_intakePos1); robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1); robot.spin2.setPower(1 - spindexer_intakePos1);
shootA = true; shootA = true;
shootB = true; shootB = true;
shootC = true; shootC = true;
@@ -764,8 +764,8 @@ public class TeleopV2 extends LinearOpMode {
public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) { public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
// Set spin positions // Set spin positions
robot.spin1.setPosition(spindexer); robot.spin1.setPower(spindexer);
robot.spin2.setPosition(1 - spindexer); robot.spin2.setPower(1 - spindexer);
// Check if spindexer has reached the target position // Check if spindexer has reached the target position
if (spinOk || getRuntime() - stamp > 1.5) { if (spinOk || getRuntime() - stamp > 1.5) {

167
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java Normal file
View File

@@ -0,0 +1,167 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@Config
@TeleOp
public class IntakeTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Servos servo;
public static int mode = 0; // 0 for teleop, 1 for auto
public static double manualPow = 0.5;
double stamp = 0;
int ticker = 0;
boolean b1 = false;
boolean b2 = false;
boolean b3 = false;
boolean steadySpin = false;
double powPID = 0.0;
double spindexerPos = spindexer_intakePos1;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
servo = new Servos(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
if (mode == 0) {
if (gamepad1.cross) {
ticker = 0;
robot.spin1.setPower(manualPow);
robot.spin2.setPower(-manualPow);
robot.intake.setPower(1);
} else {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.5) {
robot.intake.setPower(-1);
} else {
robot.intake.setPower(0);
}
}
} else if (mode == 1) {
if (gamepad1.cross){
robot.intake.setPower(1);
} else if (gamepad1.circle){
robot.intake.setPower(-1);
} else {
robot.intake.setPower(0);
}
colorDetect();
spindexer();
if (b1 && steadySpin && getRuntime() - stamp > 0.5){
if (!b2){
if (servo.spinEqual(spindexer_intakePos1)){
spindexerPos = spindexer_intakePos2;
} else if (servo.spinEqual(spindexer_intakePos2)){
spindexerPos = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos3)){
spindexerPos = spindexer_intakePos1;
}
} else if (!b3){
if (servo.spinEqual(spindexer_intakePos1)){
spindexerPos = spindexer_intakePos3;
} else if (servo.spinEqual(spindexer_intakePos2)){
spindexerPos = spindexer_intakePos1;
} else if (servo.spinEqual(spindexer_intakePos3)){
spindexerPos = spindexer_intakePos2;
}
} else {
spindexerPos = spindexer_outtakeBall1;
}
}
powPID = servo.setSpinPos(spindexerPos);
} else if (mode == 2){ // switch to this mode before switching modes
powPID = 0;
spindexerPos = spindexer_intakePos1;
stamp = getRuntime();
ticker = 0;
}
TELE.addData("Manual Power", manualPow);
TELE.addData("PID Power", powPID);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.addData("Spindex Pos", servo.getSpinPos());
}
}
public void colorDetect() {
// ----- COLOR 1 -----
double green1 = robot.color1.getNormalizedColors().green;
double blue1 = robot.color1.getNormalizedColors().blue;
double red1 = robot.color1.getNormalizedColors().red;
b1 = robot.color1.getDistance(DistanceUnit.MM) < 40;
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
// ----- COLOR 2 -----
double green2 = robot.color2.getNormalizedColors().green;
double blue2 = robot.color2.getNormalizedColors().blue;
double red2 = robot.color2.getNormalizedColors().red;
b2 = robot.color2.getDistance(DistanceUnit.MM) < 40;
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
// ----- COLOR 3 -----
double green3 = robot.color3.getNormalizedColors().green;
double blue3 = robot.color3.getNormalizedColors().blue;
double red3 = robot.color3.getNormalizedColors().red;
b3 = robot.color3.getDistance(DistanceUnit.MM) < 30;
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
TELE.update();
}
public void spindexer(){
if (!servo.spinEqual(spindexerPos)){
robot.spin1.setPower(powPID);
robot.spin2.setPower(-powPID);
steadySpin = false;
ticker = 0;
} else{
robot.spin1.setPower(0);
robot.spin2.setPower(0);
steadySpin = true;
if (ticker == 0){
stamp = getRuntime();
}
ticker++;
}
}
}

22
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java
View File

@@ -1,11 +1,9 @@
package org.firstinspires.ftc.teamcode.tests; package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.controller.PIDFController;
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.CRServo; import com.qualcomm.robotcore.hardware.CRServo;
@@ -17,14 +15,14 @@ import org.firstinspires.ftc.teamcode.utils.Robot;
@Config @Config
public class PIDServoTest extends LinearOpMode { public class PIDServoTest extends LinearOpMode {
public static double p = 0.0003, i = 0, d = 0.00001; public static double p = 2, i = 0, d = 0, f = 0;
public static double target = 0.5; public static double target = 0.5;
public static int mode = 0; //0 is for turret, 1 is for spindexer public static int mode = 0; //0 is for turret, 1 is for spindexer
public static double scalar = 1.112; public static double scalar = 1.01;
public static double restPos = 0.15; public static double restPos = 0.0;
Robot robot; Robot robot;
@@ -33,7 +31,7 @@ public class PIDServoTest extends LinearOpMode {
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
PIDController controller = new PIDController(p, i, d); PIDFController controller = new PIDFController(p, i, d, f);
controller.setTolerance(0); controller.setTolerance(0);
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
@@ -45,22 +43,22 @@ public class PIDServoTest extends LinearOpMode {
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()) { while (opModeIsActive()) {
controller.setPID(p, i, d); controller.setPIDF(p, i, d, f);
if (mode == 0) { if (mode == 0) {
pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3); pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target); double pid = controller.calculate(pos, target);
robot.turr1.setPosition(pid); robot.turr1.setPower(pid);
robot.turr2.setPosition(-pid); robot.turr2.setPower(-pid);
} else if (mode == 1) { } else if (mode == 1) {
pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3); pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target); double pid = controller.calculate(pos, target);
robot.spin1.setPosition(pid); robot.spin1.setPower(pid);
robot.spin2.setPosition(-pid); robot.spin2.setPower(-pid);
} }
telemetry.addData("pos", pos); telemetry.addData("pos", pos);

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -34,6 +34,7 @@ public class ShooterTest extends LinearOpMode {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
DcMotorEx leftShooter = robot.shooter1; DcMotorEx leftShooter = robot.shooter1;
DcMotorEx rightShooter = robot.shooter2; DcMotorEx rightShooter = robot.shooter2;
flywheel = new Flywheel();
MultipleTelemetry TELE = new MultipleTelemetry( MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry() telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -50,11 +51,9 @@ public class ShooterTest extends LinearOpMode {
leftShooter.setPower(parameter); leftShooter.setPower(parameter);
} else if (mode == 1) { } else if (mode == 1) {
double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition()); double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
rightShooter.setPower(powPID);
TELE.addData("Velocity", flywheel.getVelo()); leftShooter.setPower(powPID);
TELE.addData("PIDPower", powPID); TELE.addData("PIDPower", powPID);
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
} }
if (hoodPos != 0.501) { if (hoodPos != 0.501) {
@@ -62,8 +61,8 @@ public class ShooterTest extends LinearOpMode {
} }
if (turretPos != 0.501) { if (turretPos != 0.501) {
robot.turr1.setPosition(turretPos); robot.turr1.setPower(turretPos);
robot.turr2.setPosition(turretPos); robot.turr2.setPower(turretPos);
} }
robot.transfer.setPower(transferPower); robot.transfer.setPower(transferPower);
@@ -72,6 +71,10 @@ public class ShooterTest extends LinearOpMode {
} else { } else {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
} }
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
TELE.addData("Position", robot.shooter1.getCurrentPosition());
TELE.update(); TELE.update();

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -51,7 +51,7 @@ public class Flywheel {
velo3 = velo2; velo3 = velo2;
velo2 = velo1; velo2 = velo1;
currentPos = shooter1CurPos / 2048; currentPos = shooter1CurPos / 3072;
stamp = getTimeSeconds(); //getRuntime(); stamp = getTimeSeconds(); //getRuntime();
velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1)); velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos; initPos = currentPos;

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -13,6 +13,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
public class PositionalServoProgrammer extends LinearOpMode { public class PositionalServoProgrammer extends LinearOpMode {
Robot robot; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
Servos servo;
public static double spindexPos = 0.501; public static double spindexPos = 0.501;
public static double turretPos = 0.501; public static double turretPos = 0.501;
public static double transferPos = 0.501; public static double transferPos = 0.501;
@@ -22,16 +24,25 @@ public class PositionalServoProgrammer extends LinearOpMode {
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()){ while (opModeIsActive()){
if (spindexPos != 0.501){ if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){
robot.spin1.setPosition(spindexPos); double pos = servo.setSpinPos(spindexPos);
robot.spin2.setPosition(1-spindexPos); robot.spin1.setPower(pos);
robot.spin2.setPower(-pos);
} else{
robot.spin1.setPower(0);
robot.spin2.setPower(0);
} }
if (turretPos != 0.501){ if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
robot.turr1.setPosition(turretPos); double pos = servo.setTurrPos(turretPos);
robot.turr2.setPosition(1-turretPos); robot.turr1.setPower(pos);
robot.turr2.setPower(-pos);
} else {
robot.turr1.setPower(0);
robot.turr2.setPower(0);
} }
if (transferPos != 0.501){ if (transferPos != 0.501){
robot.transferServo.setPosition(transferPos); robot.transferServo.setPosition(transferPos);
@@ -39,14 +50,15 @@ public class PositionalServoProgrammer extends LinearOpMode {
if (hoodPos != 0.501){ if (hoodPos != 0.501){
robot.hood.setPosition(hoodPos); robot.hood.setPosition(hoodPos);
} }
TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3)); TELE.addData("spindexer", servo.getSpinPos());
TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3)); TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3)); TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3)); TELE.addData("turret", servo.getTurrPos());
TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
TELE.addData("hood voltage", robot.hoodPos.getVoltage()); TELE.addData("hood voltage", robot.hoodPos.getVoltage());
TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
TELE.addData("turret voltage", robot.turr1Pos.getVoltage()); TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
TELE.update(); TELE.update();
} }
} }

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -35,13 +36,13 @@ public class Robot {
public Servo rejecter; public Servo rejecter;
public Servo turr1; public CRServo turr1;
public Servo turr2; public CRServo turr2;
public Servo spin1; public CRServo spin1;
public Servo spin2; public CRServo spin2;
public DigitalChannel pin0; public DigitalChannel pin0;
@@ -104,6 +105,8 @@ public class Robot {
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooterEncoder = shooter1; shooterEncoder = shooter1;
@@ -111,19 +114,19 @@ public class Robot {
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos"); hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
turr1 = hardwareMap.get(Servo.class, "t1"); turr1 = hardwareMap.get(CRServo.class, "t1");
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
turr2 = hardwareMap.get(Servo.class, "t2"); turr2 = hardwareMap.get(CRServo.class, "t2");
turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos"); turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
spin1 = hardwareMap.get(Servo.class, "spin1"); spin1 = hardwareMap.get(CRServo.class, "spin1");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
spin2 = hardwareMap.get(Servo.class, "spin2"); spin2 = hardwareMap.get(CRServo.class, "spin2");
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -1,52 +1,61 @@
package org.firstinspires.ftc.teamcode.utils; package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDFController;
import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config @Config
public class Servos { public class Servos {
PIDController spinPID; Robot robot;
PIDController turretPID; PIDFController spinPID;
PIDFController turretPID;
//PID constants //PID constants
public static double spinP = 0.0, spinI = 0.0, spinD = 0.0; public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03;
public static double turrP = 0.0, turrI = 0.0, turrD = 0.0; public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF;
public static double spin_scalar = 0.15; public static double spin_scalar = 1.011;
public static double spin_restPos = 1.112; public static double spin_restPos = 0.0;
public static double turret_scalar = 0.15; public static double turret_scalar = 1.009;
public static double turret_restPos = 1.112; public static double turret_restPos = 0.0;
public void initServos() { public Servos(HardwareMap hardwareMap) {
spinPID = new PIDController(spinP, spinI, spinD); robot = new Robot(hardwareMap);
turretPID = new PIDController(turrP, turrI, turrD); spinPID = new PIDFController(spinP, spinI, spinD, spinF);
turretPID = new PIDFController(turrP, turrI, turrD, turrF);
} }
// In the code below, encoder = robot.servo.getVoltage() // In the code below, encoder = robot.servo.getVoltage()
public double getSpinPos(double encoder){ public double getSpinPos() {
return spin_scalar * ((encoder - spin_restPos) / 3.3); return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
}
public double setSpinPos(double pos, double encoder){
spinPID.setPID(spinP, spinI, spinD);
return spinPID.calculate(this.getSpinPos(encoder), pos);
}
public boolean spinEqual(double pos, double encoder){
return Math.abs(pos - this.getSpinPos(encoder)) < 0.01;
} }
public double getTurrPos(double encoder){ public double setSpinPos(double pos) {
return turret_scalar * ((encoder - turret_restPos) / 3.3); spinPID.setPIDF(spinP, spinI, spinD, spinF);
}
public double setTurrPos(double pos, double encoder){
turretPID.setPID(turrP, turrI, turrD);
return spinPID.calculate(this.getTurrPos(encoder), pos); return spinPID.calculate(this.getSpinPos(), pos);
} }
public boolean turretEqual(double pos, double encoder){
return Math.abs(pos - this.getTurrPos(encoder)) < 0.01; public boolean spinEqual(double pos) {
return Math.abs(pos - this.getSpinPos()) < 0.01;
}
public double getTurrPos() {
return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
}
public double setTurrPos(double pos) {
turretPID.setPIDF(turrP, turrI, turrD, turrF);
return spinPID.calculate(this.getTurrPos(), pos);
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < 0.01;
} }
} }