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

Alr we got teleop working but just barely....almost finished with the gridn yk?

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-07 00:10:18 -06:00
parent a278fc0489
commit 4e5f0dd43d
5 changed files with 268 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java
View File

@@ -16,7 +16,7 @@ import java.util.Objects;
@Config
public final class PinpointLocalizer implements Localizer {
public static class Params {
public double parYTicks = 3765.023079161767; // y position of the parallel encoder (in tick units)
public double parYTicks = -3765.023079161767; // y position of the parallel encoder (in tick units)
public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units)
}

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -168,7 +168,7 @@ public class Shooter implements Subsystem {
public void setTurretMode(String mode){ turretMode = mode;}
public void trackGoal(Pose2d robotPose, Pose2d goalPose, boolean shooterOn){
public double trackGoal(Pose2d robotPose, Pose2d goalPose){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -178,7 +178,7 @@ public class Shooter implements Subsystem {
Pose2d deltaPose = new Pose2d(
goalPose.position.x - robotPose.position.x,
goalPose.position.y - robotPose.position.y,
goalPose.heading.toDouble() - robotPose.heading.toDouble()
goalPose.heading.toDouble() - (robotPose.heading.toDouble())
);
double distance = Math.sqrt(
@@ -187,25 +187,19 @@ public class Shooter implements Subsystem {
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight
);
telemetry.addData("dst", distance);
double shooterPow = getPowerByDist(distance);
double hoodAngle = getAngleByDist(distance);
if (shooterOn){
fly1.setVelocity(shooterPow);
fly2.setPower(fly1.getPower());
} else {
fly1.setPower(0);
fly2.setPower(0);
}
hoodServo.setPosition(hoodAngle);
// hoodServo.setPosition(hoodAngle);
moveTurret(getTurretPosByDeltaPose(deltaPose));
return distance;
@@ -220,17 +214,38 @@ public class Shooter implements Subsystem {
double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
if (deltaAngle < -180) {
deltaAngle +=360;
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
telemetry.addData("deltaAngle", deltaAngle);
if (deltaAngle > 90) {
deltaAngle -=360;
}
deltaAngle /= (0.9974*355);
return (0.5+deltaAngle) ;
// deltaAngle += aTanAngle;
deltaAngle /= (335);
telemetry.addData("dAngle", deltaAngle);
telemetry.addData("AtanAngle", aTanAngle);
return (0.30-deltaAngle) ;
}
//62, 0.44
//56.5, 0.5
public double getPowerByDist(double dist){
//TODO: ADD LOGIC
@@ -239,8 +254,15 @@ public class Shooter implements Subsystem {
public double getAngleByDist(double dist){
//TODO: ADD LOGIC
return dist;
double newDist = dist - 56.5;
double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
return pos;
}
@@ -285,7 +307,7 @@ public class Shooter implements Subsystem {
}
if (Objects.equals(turretMode, "MANUAL")){
hoodServo.setPosition(hoodPos);
// hoodServo.setPosition(hoodPos);
moveTurret(turretPos);

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
View File

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Servo;
@@ -31,6 +33,16 @@ public class Transfer implements Subsystem{
this.motorPow = pow;
}
public void transferOut(){
this.setTransferPosition(transferServo_out);
}
public void transferIn(){
this.setTransferPosition(transferServo_in);
}
@Override

217
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -3,14 +3,17 @@ package org.firstinspires.ftc.teamcode.teleop;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -44,7 +47,7 @@ public class TeleopV1 extends LinearOpMode {
public static double power = 0.0;
public static double pos = 0.501;
public static double pos = 0.5;
ToggleButtonReader g1RightBumper;
@@ -54,15 +57,38 @@ public class TeleopV1 extends LinearOpMode {
ToggleButtonReader g2Triangle;
ToggleButtonReader g2RightBumper;
ToggleButtonReader g2LeftBumper;
ToggleButtonReader g2DpadUp;
ToggleButtonReader g2DpadDown;
public double g1RightBumperStamp = 0.0;
public double g2LeftBumperStamp = 0.0;
public static int spindexerPos = 0;
Shooter shooter;
public boolean scoreAll = false;
MecanumDrive drive ;
public boolean autotrack = true;
@Override
public void runOpMode() throws InterruptedException {
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
@@ -90,6 +116,24 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.X
);
g2RightBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.RIGHT_BUMPER
);
g2LeftBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.LEFT_BUMPER
);
g2DpadUp = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_UP
);
g2DpadDown = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_DOWN
);
drivetrain = new Drivetrain(robot, TELE, g1);
@@ -109,26 +153,161 @@ public class TeleopV1 extends LinearOpMode {
spindexer.setTelemetryOn(true);
shooter = new Shooter(robot, TELE);
shooter.setShooterMode("MANUAL");
waitForStart();
if (isStopRequested()) return;
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
while(opModeIsActive()){
drive.updatePoseEstimate();
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
robot.hood.setPosition(pos);
intake();
drivetrain.update();
TELE.update();
transfer.setTransferPower(power);
transfer.setTransferPosition(pos);
transfer.update();
g2RightBumper.readValue();
g2LeftBumper.readValue();
g2DpadDown.readValue();
g2DpadUp.readValue();
if(g2DpadUp.wasJustPressed()){
pos -=0.02;
}
if(g2DpadDown.wasJustPressed()){
pos +=0.02;
}
if (gamepad2.dpad_left){
pos = shooter.getAngleByDist(
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0))
);
}
TELE.addData("hood", pos);
if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0));
} else {
autotrack = false;
shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.04);
}
if (gamepad2.right_stick_button){
autotrack = true;
}
if (g2RightBumper.wasJustPressed()){
transfer.setTransferPower(1);
transfer.transferIn();
shooter.setManualPower(1);
}
if (g2RightBumper.wasJustReleased()){
transfer.setTransferPower(1);
transfer.transferOut();
}
if (gamepad2.left_stick_y>0.5){
shooter.setManualPower(0);
} else if (gamepad2.left_stick_y<-0.5){
shooter.setManualPower(1);
}
if (g2LeftBumper.wasJustPressed()){
g2LeftBumperStamp = getRuntime();
scoreAll = true;
}
if (scoreAll) {
double time = getRuntime() - g2LeftBumperStamp;
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-2, 0, 0));
shooter.setManualPower(1);
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1.5){
spindexer.outtake3();
} else if (time < 2.0){
transfer.transferIn();
} else if (time < 3){
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.5){
transfer.transferIn();
} else if (time < 4.5){
transfer.transferOut();
spindexer.outtake1();
} else if (time < 5){
transfer.transferIn();
} else {
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
}
shooter.update();
@@ -156,6 +335,8 @@ public class TeleopV1 extends LinearOpMode {
if (g1RightBumper.wasJustPressed()){
shooter.setManualPower(0);
if (getRuntime() - g1RightBumperStamp < 0.3){
intake.reverse();
@@ -164,6 +345,8 @@ public class TeleopV1 extends LinearOpMode {
}
spindexer.intake();
transfer.transferOut();
g1RightBumperStamp = getRuntime();
@@ -177,6 +360,9 @@ public class TeleopV1 extends LinearOpMode {
if (g2Circle.wasJustPressed()){
spindexer.outtake3();
}
if (g2Triangle.wasJustPressed()){
@@ -187,8 +373,27 @@ public class TeleopV1 extends LinearOpMode {
spindexer.outtake1();
}
if (g2Circle.wasJustReleased()){
transfer.setTransferPower(1);
}
if (g2Triangle.wasJustReleased()){
transfer.setTransferPower(1);
}
if (g2Square.wasJustReleased()){
transfer.setTransferPower(1);
}
}
intake.update();

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

@@ -126,6 +126,8 @@ public class Robot {
transferServo = hardwareMap.get(Servo.class, "transferServo");
transfer.setDirection(DcMotorSimple.Direction.REVERSE);
}
}