diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java new file mode 100644 index 0000000..9c2838e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.reorg; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public interface AutonomousCommand { + public void execute(boolean value, Telemetry telemetry); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java new file mode 100644 index 0000000..c6f8eb1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.reorg; + +public class AutonomousConstant { + + static final double COUNTS_PER_MOTOR_REV = 537.6; // eg: TETRIX Motor Encoder + static final double DRIVE_GEAR_REDUCTION = 1.0; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 3.77953; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * Math.PI); + static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); + static final double DRIVE_SPEED = 0.3; + static final double TURN_SPEED = 0.4; + + static final double LONG_TIMEOUT = 1000; + static final double DEGREE_TOO_DISTANCE = 0.21944444444; + static final double ARM_SPEED = .1; + static final double TICKS_TO_DEGREES = 0.07462686567; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java new file mode 100644 index 0000000..2a57a11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.reorg; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@Autonomous(name="auto-test-facade", group="Robot") +public class AutonomousTestFacade extends LinearOpMode { + + @Override + public void runOpMode() { + /* + Class where all our initialization code lives + To use it, we just instantiate a copy of it! + */ + AutonomousTestService service = new AutonomousTestService(hardwareMap); + + /* + Initialize the hardware stuff + */ + service.initializeHardware(); // good + + /* + There's a sleep here, need to determine if it's needed + sleep(1000); + */ + + + /* + Initialize the direction stuff + */ + service.initializeDirection(); // good + + /* + Stop and reset all encoders + */ + service.initializeEncoders(); // good + + /* + Update telemetry + */ + telemetry.update(); + + /* + From the JavaDoc: Pauses the Linear Op Mode until start has been pressed + or until the current thread is interrupted. + */ + waitForStart(); + { + service.execute(opModeIsActive(), telemetry); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java new file mode 100644 index 0000000..2d0f9b4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java @@ -0,0 +1,240 @@ +package org.firstinspires.ftc.teamcode.reorg; + +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class AutonomousTestService implements AutonomousCommand { + + private DcMotor leftDrive; + private DcMotor rightDrive; + private DcMotor backRightDrive; + private DcMotor backLeftDrive; + private ColorSensor colorRight; + private ColorSensor colorLeft; + private Servo wrist; + private Servo gripper; + private DcMotor arm; + private ElapsedTime runTime; + + public AutonomousTestService(HardwareMap hwMap) { + this.runTime = new ElapsedTime(); + leftDrive = hwMap.get(DcMotor.class, "Drive front lt"); + rightDrive = hwMap.get(DcMotor.class, "Drive front rt"); + backLeftDrive = hwMap.get(DcMotor.class, "Drive back lt"); + backRightDrive = hwMap.get(DcMotor.class, "Drive back rt"); + colorRight = hwMap.get(ColorSensor.class, "color right"); + colorLeft = hwMap.get(ColorSensor.class, "color left"); + arm = hwMap.get(DcMotor.class, "arm raise"); + wrist = hwMap.get(Servo.class, "wrist"); + gripper = hwMap.get(Servo.class, "gripper"); + } + + public void initializeHardware() { + setWristPosition(1); + setGripperPosition(1); + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + private void setWristPosition(int position) { + wrist.setPosition(position); + } + + private void setGripperPosition(int position) { + wrist.setPosition(position); + } + + public void initializeDirection() { + setDriveDirectionForward(); + setArmDirection(DcMotor.Direction.REVERSE); + } + + public void initializeEncoders() { + setDriveModeStopAndResetEncoder(); + setDriveModeRunUsingEncoder(); + setArmDriveModeStopAndResetEncoder(); + setArmDriveModeRunUsingEncoder(); + } + + private void setDriveDirectionForward() { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + } + + private void setDriveDirectionLeft() { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + } + + private void setDriveDirectionStraightLeft() { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + } + + private void turn(double degrees, boolean isOpModeActive, Telemetry telemetry) { + double turning_distance = degrees * AutonomousConstant.DEGREE_TOO_DISTANCE; + move(turning_distance, isOpModeActive, telemetry); + } + + private void move(double degrees, boolean isOpModeActive, Telemetry telemetry) { + encoderDrive(AutonomousConstant.DRIVE_SPEED, degrees, degrees, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry); + } + + private void setDriveMode(DcMotor.RunMode runMode) { + leftDrive.setMode(runMode); + rightDrive.setMode(runMode); + backLeftDrive.setMode(runMode); + backRightDrive.setMode(runMode); + } + private void setArmMode(DcMotor.RunMode runMode) { + arm.setMode(runMode); + } + + public void setArmDirection(DcMotor.Direction direction) { + arm.setDirection(direction); + } + + public void setDriveModeStopAndResetEncoder() { + setDriveMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } + + public void setDriveModeRunUsingEncoder() { + setDriveMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + public void setArmDriveModeStopAndResetEncoder() { + setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } + + public void setArmDriveModeRunUsingEncoder() { + setArmMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + private void setDriveTargetPosition(int nlt, int nrt, int nbrt, int nblt) { + leftDrive.setTargetPosition(nlt); + rightDrive.setTargetPosition(nrt); + backRightDrive.setTargetPosition(nbrt); + backLeftDrive.setTargetPosition(nblt); + } + + public void setDriveModeRunToPosition() { + setDriveMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + public void setDrivePower(double speed) { + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + backRightDrive.setPower(Math.abs(speed)); + backLeftDrive.setPower(Math.abs(speed)); + } + + public int getColorSensorReadingOnRightSide(Telemetry telemetry) { + telemetry.addData("Clear Right", colorRight.alpha()); + telemetry.addData("Red Right", colorRight.red()); + telemetry.addData("Green Right", colorRight.green()); + telemetry.addData("Blue Right", colorRight.blue()); + telemetry.addData("Color Sensor","right"); + return colorRight.red(); + } + + public int getColorSensorReadingOnLeftSide(Telemetry telemetry) { + telemetry.addData("Clear Left", colorLeft.alpha()); + telemetry.addData("Red Left ", colorLeft.red()); + telemetry.addData("Green Left", colorLeft.green()); + telemetry.addData("Blue Left", colorLeft.blue()); + return colorLeft.red(); + } + + private int calculateCurrentPosition(int position, double distance) { + return position + (int) (distance * AutonomousConstant.COUNTS_PER_INCH); + } + + private void drive(double distance, boolean isOpModeActive, Telemetry telemetry) { + encoderDrive(AutonomousConstant.DRIVE_SPEED, distance, distance, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry); + } + + private void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean isOpModeActive, Telemetry telemetry) { + + if (isOpModeActive) { + + int ldgcp = leftDrive.getCurrentPosition(); + int rdgcp = leftDrive.getCurrentPosition(); + int bldgcp = leftDrive.getCurrentPosition(); + int brdgcp = leftDrive.getCurrentPosition(); + + int nlt = calculateCurrentPosition(ldgcp, leftInches); + int nrt = calculateCurrentPosition(rdgcp, rightInches); + int nblt = calculateCurrentPosition(bldgcp, leftInches); + int nbrt = calculateCurrentPosition(brdgcp, rightInches); + + setDriveTargetPosition(nlt, nrt, nblt, nbrt); + + setDriveModeRunToPosition(); + + runTime.reset(); + + setDrivePower(speed); + + while (isOpModeActive && (runTime.seconds() < timeout) && (leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.isBusy() && backRightDrive.isBusy())) { + telemetry.addData("Running to", " %7d :%7d", nlt, nrt); + telemetry.addData("Currently at", " at %7d :%7d", ldgcp, rdgcp, bldgcp, brdgcp); + telemetry.update(); + } + + setDrivePower(0); + + setDriveModeRunUsingEncoder(); + + } + + } + + @Override + public void execute(boolean isOpModeActive, Telemetry telemetry) { + /* + Do we need this? Can it be part of the initialization? + */ + // arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + /* + Drive forward to left/right/center placement of item + */ + setDriveDirectionForward(); + drive(26, isOpModeActive, telemetry); + + + + if (getColorSensorReadingOnLeftSide(telemetry) > 50) { + + if(getColorSensorReadingOnLeftSide(telemetry) > getColorSensorReadingOnRightSide(telemetry)) + telemetry.addData("color sensor","left"); + + setDriveDirectionLeft(); + turn(90, isOpModeActive, telemetry); + + setDriveDirectionStraightLeft(); + move(8.5, isOpModeActive, telemetry); + + } + + + if (getColorSensorReadingOnRightSide(telemetry) > 50) { + + if(getColorSensorReadingOnRightSide(telemetry) > getColorSensorReadingOnLeftSide(telemetry)) + telemetry.addData("color sensor","right"); + + } + + + } +}