daniel files added
This commit is contained in:
@@ -1,4 +0,0 @@
|
|||||||
package org.firstinspires.ftc.teamcode.constants;
|
|
||||||
|
|
||||||
public class blank {
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,144 @@
|
|||||||
|
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.teamcode.utils.Robot;
|
||||||
|
|
||||||
|
|
||||||
|
@TeleOp
|
||||||
|
@Config
|
||||||
|
public class ActiveColorSensorTest extends LinearOpMode {
|
||||||
|
Robot robot;
|
||||||
|
MultipleTelemetry TELE;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException{
|
||||||
|
robot = new Robot(hardwareMap);
|
||||||
|
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
int b1Purple = 1;
|
||||||
|
int b1Total = 1;
|
||||||
|
int b2Purple = 1;
|
||||||
|
int b2Total = 1;
|
||||||
|
int b3Purple = 1;
|
||||||
|
int b3Total = 1;
|
||||||
|
|
||||||
|
double totalStamp1 = 0.0;
|
||||||
|
double purpleStamp1 = 0.0;
|
||||||
|
double totalStamp2 = 0.0;
|
||||||
|
double purpleStamp2 = 0.0;
|
||||||
|
double totalStamp3 = 0.0;
|
||||||
|
double purpleStamp3 = 0.0;
|
||||||
|
|
||||||
|
String b1 = "none";
|
||||||
|
String b2 = "none";
|
||||||
|
String b3 = "none";
|
||||||
|
|
||||||
|
double position = 0.0;
|
||||||
|
|
||||||
|
double stamp = getRuntime();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
while (opModeIsActive()){
|
||||||
|
|
||||||
|
if ((getRuntime() % 0.3) >0.15) {
|
||||||
|
position = spindexer_intakePos1 + 0.015;
|
||||||
|
} else {
|
||||||
|
position = spindexer_intakePos1 - 0.015;
|
||||||
|
}
|
||||||
|
robot.spin1.setPosition(position);
|
||||||
|
robot.spin2.setPosition(1-position);
|
||||||
|
|
||||||
|
robot.intake.setPower(1);
|
||||||
|
|
||||||
|
// Reset the counters after 1 second of not reading a ball.
|
||||||
|
final double ColorCounterResetDelay = 1.0;
|
||||||
|
// Number of times the loop needs to run before deciding on a color.
|
||||||
|
final int ColorCounterTotalMinCount = 20;
|
||||||
|
// If the color sensor reads a color this percentage of time
|
||||||
|
// out of the total, declare the color.
|
||||||
|
// Usage: (Color Count)/(Total Count) > ColorCounterThreshold
|
||||||
|
final double ColorCounterThreshold = 0.65;
|
||||||
|
|
||||||
|
if (robot.pin1.getState()){
|
||||||
|
if (robot.pin0.getState()){
|
||||||
|
b1Purple ++;
|
||||||
|
}
|
||||||
|
b1Total++;
|
||||||
|
totalStamp1 = getRuntime();
|
||||||
|
}
|
||||||
|
if (getRuntime() - totalStamp1 > ColorCounterResetDelay) {
|
||||||
|
// Too Much time has passed without detecting ball
|
||||||
|
b1 = "none";
|
||||||
|
b1Total = 1;
|
||||||
|
b1Purple = 1;
|
||||||
|
}else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){
|
||||||
|
// Enough Time has passed and we met the threshold
|
||||||
|
b1 = "Purple";
|
||||||
|
}else if (b1Total > ColorCounterTotalMinCount) {
|
||||||
|
// Enough Time passed WITHOUT meeting the threshold
|
||||||
|
b1 = "Green";
|
||||||
|
}
|
||||||
|
|
||||||
|
if (robot.pin3.getState()){
|
||||||
|
if (robot.pin2.getState()){
|
||||||
|
b2Purple ++;
|
||||||
|
}
|
||||||
|
b2Total++;
|
||||||
|
totalStamp2 = getRuntime();
|
||||||
|
}
|
||||||
|
if (getRuntime() - totalStamp2 > ColorCounterResetDelay) {
|
||||||
|
// Too Much time has passed without detecting ball
|
||||||
|
b2 = "none";
|
||||||
|
b2Total = 1;
|
||||||
|
b2Purple = 1;
|
||||||
|
}else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){
|
||||||
|
// Enough Time has passed and we met the threshold
|
||||||
|
b2 = "Purple";
|
||||||
|
}else if (b2Total > ColorCounterTotalMinCount) {
|
||||||
|
// Enough Time passed WITHOUT meeting the threshold
|
||||||
|
b2 = "Green";
|
||||||
|
}
|
||||||
|
|
||||||
|
if (robot.pin5.getState()){
|
||||||
|
if (robot.pin4.getState()){
|
||||||
|
b3Purple ++;
|
||||||
|
}
|
||||||
|
b3Total++;
|
||||||
|
totalStamp3 = getRuntime();
|
||||||
|
}
|
||||||
|
if (getRuntime() - totalStamp3 > ColorCounterResetDelay) {
|
||||||
|
// Too Much time has passed without detecting ball
|
||||||
|
b3 = "none";
|
||||||
|
b3Total = 1;
|
||||||
|
b3Purple = 1;
|
||||||
|
}else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){
|
||||||
|
// Enough Time has passed and we met the threshold
|
||||||
|
b3 = "Purple";
|
||||||
|
}else if (b3Total > ColorCounterTotalMinCount) {
|
||||||
|
// Enough Time passed WITHOUT meeting the threshold
|
||||||
|
b3 = "Green";
|
||||||
|
}
|
||||||
|
|
||||||
|
TELE.addData("Green1:", robot.pin1.getState());
|
||||||
|
TELE.addData("Purple1:", robot.pin0.getState());
|
||||||
|
TELE.addData("Green2:", robot.pin3.getState());
|
||||||
|
TELE.addData("Purple2:", robot.pin2.getState());
|
||||||
|
TELE.addData("Green3:", robot.pin5.getState());
|
||||||
|
TELE.addData("Purple3:", robot.pin4.getState());
|
||||||
|
TELE.addData("1", b1);
|
||||||
|
TELE.addData("2",b2);
|
||||||
|
TELE.addData("3",b3);
|
||||||
|
|
||||||
|
TELE.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1,63 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode.tests;
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
@TeleOp
|
||||||
|
@Config
|
||||||
|
public class ColorSensorTest extends LinearOpMode {
|
||||||
|
Robot robot;
|
||||||
|
MultipleTelemetry TELE;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
robot = new Robot(hardwareMap);
|
||||||
|
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
// ----- COLOR 1 -----
|
||||||
|
double green1 = robot.color1.getNormalizedColors().green;
|
||||||
|
double blue1 = robot.color1.getNormalizedColors().blue;
|
||||||
|
double red1 = robot.color1.getNormalizedColors().red;
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1,210 @@
|
|||||||
|
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 com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
|
||||||
|
|
||||||
|
@TeleOp
|
||||||
|
@Config
|
||||||
|
public class ShooterTest extends LinearOpMode {
|
||||||
|
|
||||||
|
Robot robot;
|
||||||
|
|
||||||
|
public static double pow = 0.0;
|
||||||
|
public static double vel = 0.0;
|
||||||
|
public static double ecpr = 1024.0; // CPR of the encoder
|
||||||
|
public static double hoodPos = 0.5;
|
||||||
|
public static double turretPos = 0.9;
|
||||||
|
|
||||||
|
public static String flyMode = "VEL";
|
||||||
|
|
||||||
|
public static boolean AutoTrack = false;
|
||||||
|
|
||||||
|
double initPos = 0.0;
|
||||||
|
|
||||||
|
double velo = 0.0;
|
||||||
|
double velo1 = 0.0;
|
||||||
|
double velo2 = 0.0;
|
||||||
|
double velo3 = 0.0;
|
||||||
|
double velo4 = 0.0;
|
||||||
|
double velo5 = 0.0;
|
||||||
|
|
||||||
|
double stamp1 = 0.0;
|
||||||
|
|
||||||
|
double initPos1 = 0.0;
|
||||||
|
|
||||||
|
double powPID = 0.0;
|
||||||
|
|
||||||
|
public static int maxVel = 4500;
|
||||||
|
|
||||||
|
public static boolean shoot = false;
|
||||||
|
|
||||||
|
public static int spindexPos = 1;
|
||||||
|
|
||||||
|
public static boolean intake = true;
|
||||||
|
|
||||||
|
public static int tolerance = 50;
|
||||||
|
|
||||||
|
double stamp = 0.0;
|
||||||
|
|
||||||
|
public static double kP = 0.001; // small proportional gain (tune this)
|
||||||
|
public static double maxStep = 0.06; // prevents sudden jumps
|
||||||
|
public static double distance = 50;
|
||||||
|
|
||||||
|
MultipleTelemetry TELE;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() throws InterruptedException {
|
||||||
|
|
||||||
|
robot = new Robot(hardwareMap);
|
||||||
|
|
||||||
|
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
|
||||||
|
Shooter shooter = new Shooter(robot, TELE);
|
||||||
|
|
||||||
|
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
|
|
||||||
|
shooter.setTelemetryOn(true);
|
||||||
|
|
||||||
|
shooter.setShooterMode(flyMode);
|
||||||
|
|
||||||
|
initPos = shooter.getECPRPosition();
|
||||||
|
|
||||||
|
int ticker = 0;
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
|
||||||
|
if (isStopRequested()) return;
|
||||||
|
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
ticker++;
|
||||||
|
|
||||||
|
if (AutoTrack){
|
||||||
|
hoodPos = hoodAnglePrediction(distance);
|
||||||
|
vel = velPrediction(distance);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
shooter.setShooterMode(flyMode);
|
||||||
|
|
||||||
|
shooter.setManualPower(pow);
|
||||||
|
|
||||||
|
robot.hood.setPosition(hoodPos);
|
||||||
|
robot.turr1.setPosition(turretPos);
|
||||||
|
robot.turr2.setPosition(1 - turretPos);
|
||||||
|
if (intake) {
|
||||||
|
robot.transfer.setPower(0);
|
||||||
|
robot.intake.setPower(0.75);
|
||||||
|
robot.spin1.setPosition(spindexer_intakePos1);
|
||||||
|
robot.spin2.setPosition(1 - spindexer_intakePos1);
|
||||||
|
} else {
|
||||||
|
robot.transfer.setPower(.75 + (powPID/4));
|
||||||
|
robot.intake.setPower(0);
|
||||||
|
if (spindexPos == 1) {
|
||||||
|
robot.spin1.setPosition(spindexer_outtakeBall1);
|
||||||
|
robot.spin2.setPosition(1 - spindexer_outtakeBall1);
|
||||||
|
} else if (spindexPos == 2) {
|
||||||
|
robot.spin1.setPosition(spindexer_outtakeBall2);
|
||||||
|
robot.spin2.setPosition(1 - spindexer_outtakeBall2);
|
||||||
|
} else if (spindexPos == 3) {
|
||||||
|
robot.spin1.setPosition(spindexer_outtakeBall3);
|
||||||
|
robot.spin2.setPosition(1 - spindexer_outtakeBall3);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double penguin = 0;
|
||||||
|
if (ticker % 8 ==0){
|
||||||
|
penguin = shooter.getECPRPosition();
|
||||||
|
stamp = getRuntime();
|
||||||
|
velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
|
||||||
|
initPos1 = penguin;
|
||||||
|
stamp1 = stamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
velo = velo1;
|
||||||
|
|
||||||
|
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
|
||||||
|
|
||||||
|
if (vel > 500){
|
||||||
|
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- PROPORTIONAL CORRECTION ---
|
||||||
|
double error = vel - velo1;
|
||||||
|
double correction = kP * error;
|
||||||
|
|
||||||
|
// limit how fast power changes (prevents oscillation)
|
||||||
|
correction = Math.max(-maxStep, Math.min(maxStep, correction));
|
||||||
|
|
||||||
|
// --- FINAL MOTOR POWER ---
|
||||||
|
powPID = feed + correction;
|
||||||
|
|
||||||
|
// clamp to allowed range
|
||||||
|
powPID = Math.max(0, Math.min(1, powPID));
|
||||||
|
|
||||||
|
if (vel - velo > 1000){
|
||||||
|
powPID = 1;
|
||||||
|
} else if (velo - vel > 1000){
|
||||||
|
powPID = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
shooter.setVelocity(powPID);
|
||||||
|
|
||||||
|
if (shoot) {
|
||||||
|
robot.transferServo.setPosition(transferServo_in);
|
||||||
|
} else {
|
||||||
|
robot.transferServo.setPosition(transferServo_out);
|
||||||
|
}
|
||||||
|
|
||||||
|
shooter.update();
|
||||||
|
|
||||||
|
TELE.addData("Revolutions", shooter.getECPRPosition());
|
||||||
|
TELE.addData("hoodPos", shooter.gethoodPosition());
|
||||||
|
TELE.addData("turretPos", shooter.getTurretPosition());
|
||||||
|
TELE.addData("Power Fly 1", robot.shooter1.getPower());
|
||||||
|
TELE.addData("Power Fly 2", robot.shooter2.getPower());
|
||||||
|
TELE.addData("powPID", shooter.getpowPID());
|
||||||
|
TELE.addData("Velocity", velo);
|
||||||
|
TELE.update();
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public double hoodAnglePrediction(double distance) {
|
||||||
|
double L = 0.298317;
|
||||||
|
double A = 1.02124;
|
||||||
|
double k = 0.0157892;
|
||||||
|
double n = 3.39375;
|
||||||
|
|
||||||
|
double dist = Math.sqrt(distance*distance+24*24);
|
||||||
|
|
||||||
|
return L + A * Math.exp(-Math.pow(k * dist, n));
|
||||||
|
}
|
||||||
|
public static double velPrediction(double distance) {
|
||||||
|
|
||||||
|
double x = Math.sqrt(distance*distance+24*24);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
double A = -211149.992;
|
||||||
|
double B = -1.19943;
|
||||||
|
double C = 3720.15909;
|
||||||
|
|
||||||
|
return A * Math.pow(x, B) + C;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user