TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

More work on Pizza bot's teleop- Launcher works, beginning the distance sensor coding

Browse Source
This commit is contained in:
NerdyBirdy460
2024-01-01 20:47:15 -06:00
parent ad68fbb7c0
commit 1dd8051202
2 changed files with 52 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
View File

@@ -19,7 +19,7 @@ public class SodiPizzaMinibotObject extends Robot {
public HardwareMap hardwareMap;
public DcMotor leftFront, rightFront, leftBack, rightBack;
public Servo shoulder, gripper;
public Servo shoulder, gripper, launcher;
public IMU imu;
public Rev2mDistanceSensor distSensor;
private String string;
@@ -72,6 +72,7 @@ public class SodiPizzaMinibotObject extends Robot {
//Servo Defining
shoulder = engine.hardwareMap.servo.get("arm");
gripper = engine.hardwareMap.servo.get("gripper");
launcher = engine.hardwareMap.servo.get("launcher");
//Distance Sensor

56
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
View File

@@ -9,6 +9,7 @@ import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.GRIPPER
import com.qualcomm.robotcore.hardware.DcMotor;
import org.checkerframework.checker.units.qual.A;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
@@ -20,12 +21,15 @@ import dev.cyberarm.engine.V2.GamepadChecker;
public class SodiPizzaTeleOPState extends CyberarmState {
final private SodiPizzaMinibotObject robot;
private long lastMoveTime;
private long lastMoveTime, lastDistRead /* <- last time Distance was read*/;
public double drivePower;
public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
public double lastToldAngle /* <- The angle the bot was last told to stop at */;
private int armPos;
public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
private int armPos, objectPos;
private boolean droneLaunched;
private char buttonPressed;
private GamepadChecker gp1checker, gp2checker;
YawPitchRollAngles imuInitAngle;
@@ -34,12 +38,24 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.setup();
}
public float getApproxObjPos() {
if (System.currentTimeMillis() - lastDistRead >= 500) {
/*Pseudocode: take objData1, wait, take 2, wait, take 3*/
}
approxObjPos = (objData1 + objData2 + objData3)/3;
return approxObjPos;
}
@Override
public void telemetry() {
engine.telemetry.addData("Arm should be at Position ", armPos);
engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
engine.telemetry.addData("Button Pressed = ", buttonPressed);
engine.telemetry.addLine();
engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
engine.telemetry.addData("Drone Launched?", droneLaunched);
}
@Override
@@ -50,12 +66,15 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.leftBack.setPower(0);
robot.rightBack.setPower(0);
robot.launcher.setPosition(0);
droneLaunched = false;
robot.imu.resetYaw();
imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
gp1checker = new GamepadChecker(engine, engine.gamepad1);
gp2checker = new GamepadChecker(engine, engine.gamepad2);
lastMoveTime = System.currentTimeMillis();
armPos = 0;
@@ -139,6 +158,30 @@ public class SodiPizzaTeleOPState extends CyberarmState {
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
if (engine.gamepad2.left_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.launcher.setPosition(0.98);
lastMoveTime = System.currentTimeMillis();
}
} else if (engine.gamepad2.right_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 100) {
robot.launcher.setPosition(robot.launcher.getPosition() - 0.2);
lastMoveTime = System.currentTimeMillis();
}
} else if (robot.launcher.getPosition() >= 0.95) {
if (System.currentTimeMillis() - lastMoveTime >= 1000) {
droneLaunched = true;
lastMoveTime = System.currentTimeMillis();
}
}
if (!engine.gamepad2.left_stick_button && droneLaunched) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {
robot.launcher.setPosition(robot.launcher.getPosition() - 0.025);
lastMoveTime = System.currentTimeMillis();
}
}
// This moves the arm to Collect position, which is at servo position 0.00.
if (engine.gamepad2.a && !engine.gamepad2.start) {
armPos = 1;
@@ -274,7 +317,8 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.gripper.setPosition(GRIPPER_CLOSED);
}
gp1checker.update();
gp2checker.update();
}