TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-16 04:12:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Adding run-to on arm heights and telemetry, finished all 4 junction heights, overrides, etc.

Browse Source
This commit is contained in:
Sodi
2022-12-27 20:42:00 -06:00
parent 2279a3488a
commit d6652cc807
2 changed files with 22 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -46,7 +46,7 @@ public class PhoenixBot1 {
public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance; public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder; public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft;
public CRServo collectorLeft, collectorRight; public CRServo collectorLeft, collectorRight;
@@ -144,12 +144,18 @@ public class PhoenixBot1 {
// Dead Wheel encoder for driving // Dead Wheel encoder for driving
OdometerEncoder = engine.hardwareMap.dcMotor.get("odometer encoder"); OdometerEncoder = engine.hardwareMap.dcMotor.get("odometerEncoderR");
OdometerEncoder.setDirection(DcMotorSimple.Direction.REVERSE); OdometerEncoder.setDirection(DcMotorSimple.Direction.REVERSE);
OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
OdometerEncoderLeft = engine.hardwareMap.dcMotor.get("odometerEncoderL");
OdometerEncoderLeft.setDirection(DcMotorSimple.Direction.REVERSE);
OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
HighRiserLeft.setDirection(Servo.Direction.REVERSE); HighRiserLeft.setDirection(Servo.Direction.REVERSE);
HighRiserRight.setDirection(Servo.Direction.FORWARD); HighRiserRight.setDirection(Servo.Direction.FORWARD);

45
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -4,6 +4,7 @@ import android.annotation.SuppressLint;
import android.widget.Switch; import android.widget.Switch;
import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
@@ -19,7 +20,7 @@ public class PhoenixTeleOPState extends CyberarmState {
private int CyclingArmUpAndDown = 0; private int CyclingArmUpAndDown = 0;
private double RobotRotation; private double RobotRotation;
private double RotationTarget, DeltaRotation; private double RotationTarget, DeltaRotation;
private double MinimalPower = 0.3; private double MinimalPower = 0.25;
private GamepadChecker gamepad1Checker, gamepad2Checker; private GamepadChecker gamepad1Checker, gamepad2Checker;
private int OCD; private int OCD;
@@ -33,9 +34,9 @@ public class PhoenixTeleOPState extends CyberarmState {
} else if (RotationTarget <= 0 && RobotRotation <= 0) { } else if (RotationTarget <= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation); DeltaRotation = Math.abs(RotationTarget - RobotRotation);
} else if (RotationTarget >= 0 && RobotRotation <= 0) { } else if (RotationTarget >= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget + RobotRotation); DeltaRotation = (RotationTarget + Math.abs(RobotRotation));
} else if (RotationTarget <= 0 && RobotRotation >= 0) { } else if (RotationTarget <= 0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RobotRotation + RotationTarget); DeltaRotation = (RobotRotation + Math.abs(RotationTarget));
} }
} }
@@ -53,6 +54,8 @@ public class PhoenixTeleOPState extends CyberarmState {
engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Odometer Encoder, Right", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addData("Odometer Encoder, Left", robot.OdometerEncoderLeft.getCurrentPosition());
// engine.telemetry.addData("OCD", OCD); // engine.telemetry.addData("OCD", OCD);
} }
@@ -66,7 +69,8 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.LowRiserRight.setPosition(0.45); robot.LowRiserRight.setPosition(0.45);
robot.HighRiserLeft.setPosition(0.45); robot.HighRiserLeft.setPosition(0.45);
robot.HighRiserRight.setPosition(0.45); robot.HighRiserRight.setPosition(0.45);
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
@@ -77,6 +81,11 @@ public class PhoenixTeleOPState extends CyberarmState {
@Override @Override
public void exec() { public void exec() {
if (engine.gamepad1.start && !engine.gamepad1.a) {
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
}
if (engine.gamepad1.right_trigger > 0) { if (engine.gamepad1.right_trigger > 0) {
drivePower = engine.gamepad1.right_trigger; drivePower = engine.gamepad1.right_trigger;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
@@ -171,32 +180,6 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
} }
if (engine.gamepad1.dpad_left) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -45;
CalculateDeltaRotation();
if (RobotRotation > -45 && RobotRotation <= 135) {//CCW
drivePower = (-1 * DeltaRotation / 180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -45 || RobotRotation > 136) {//CW
drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -44 && RobotRotation > -46) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.x) { if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getAngularOrientation().firstAngle;
@@ -263,7 +246,7 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(-drivePower);
} }
if (RobotRotation < -135 || RobotRotation > 44) {//CW if (RobotRotation < -135 || RobotRotation > 46) {//CW
drivePower = (1 * DeltaRotation / 180) + MinimalPower; drivePower = (1 * DeltaRotation / 180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);