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

added PSP

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-02 19:46:15 -06:00
parent 873d0c5134
commit fba9c7b114
1 changed files with 53 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java Normal file
View File

@@ -0,0 +1,53 @@
package org.firstinspires.ftc.teamcode.utils;
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;
@TeleOp
@Config
public class PositionalServoProgrammer extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
public static double spindexPos = 0.501;
public static double turretPos = 0.501;
public static double transferPos = 0.501;
public static double hoodPos = 0.501;
public static double scalar = 1.112;
public static double restPos = 0.158;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if (spindexPos != 0.501){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
if (turretPos != 0.501){
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1-turretPos);
}
if (transferPos != 0.501){
robot.transferServo.setPosition(transferPos);
}
if (hoodPos != 0.501){
robot.hood.setPosition(hoodPos);
}
TELE.addData("spindexer", scalar*((robot.spin1Pos.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("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
TELE.addData("hoodA", robot.hoodPos.getVoltage());
TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
TELE.addData("turretA", robot.turr1Pos.getVoltage());
TELE.update();
}
}
}