SCDS/FtcRobotController
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adding sample code base, still work in progress

Browse Source
This commit is contained in:
Carlos
2023-11-09 14:21:37 -08:00
parent 8aef2037c5
commit 4b6fd66770
4 changed files with 317 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java Normal file
View File

@ -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);
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java Normal file
View File

@ -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;
}

52
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java Normal file
View File

@ -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);
}
}
}

240
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java Normal file
View File

@ -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");
}
}
}