eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix all errors, first working code used for meet 1, after meet added new keybinds and some minor changes plus lots fo math, logic, adn macros added.

This commit is contained in:
UntitledRice 2023-11-11 18:39:00 -06:00
parent d7c89b9a23
commit dc48b5ebf3
23 changed files with 770 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/build.gradle
View File

@ -42,4 +42,6 @@ dependencies {
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'org.openftc:easyopencv:1.7.0'
implementation 'com.acmerobotics.roadrunner:core:0.5.5'
implementation 'org.ftclib.ftclib:vision:2.0.1'
implementation 'org.ftclib.ftclib:core:2.0.1'
}

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java Normal file
View File

@ -0,0 +1,93 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@Config
public class Arm {
//make each servo
private Servo dServo;
private Servo rAServo;
private Servo lAServo;
private Servo wristServo;
private SlidePos.SPosition pos = SlidePos.SPosition.DOWN;
private PController armController;
private double armControllerTarget;
private double ARM_KP = 0.001;
public Arm(HardwareMap hardwareMap) {
wristServo = hardwareMap.get(Servo.class, "Wrist Servo");
dServo = hardwareMap.get(Servo.class, "Door Servo");
lAServo = hardwareMap.get(Servo.class, "Left Arm Servo");
rAServo = hardwareMap.get(Servo.class, "Right Arm Servo ");
// lAServo.setDirection(Servo.Direction.REVERSE);
rAServo.setDirection(Servo.Direction.REVERSE);
dServo.setDirection(Servo.Direction.REVERSE);
// wristServo.setDirection(Servo.Direction.REVERSE);
}
public static double armScorePos = 1;
public static double doorOpenpos = 0.5;
public static double doorClosePos = 0.85;
public static double startingWristPos = 0.735;
public static double startingArmPos = 0.325;
public static double safteyDownPos = 0.4;
public static double safteyUpPos = 0.9;
public static double wristScorePos = 0.98;
public static double safeWrist = 0.8;
public void openDoor(DoorPos.DoorPosition pos) {
if (pos == DoorPos.DoorPosition.OPEN) {
dServo.setPosition(doorOpenpos);
} else if (pos == DoorPos.DoorPosition.CLOSE) {
dServo.setPosition(doorClosePos);
}
}
public void setPos(ArmPos.APosition tape) {
if (tape == ArmPos.APosition.SDOWN) {
this.armControllerTarget = startingArmPos;
lAServo.setPosition(startingArmPos);
rAServo.setPosition(startingArmPos);
} else if (tape == ArmPos.APosition.SCORE) {
this.armControllerTarget = armScorePos;
lAServo.setPosition(armScorePos);
rAServo.setPosition(armScorePos);
wristServo.setPosition(wristScorePos);
} else if (tape == ArmPos.APosition.SAFTEYDOWN) {
lAServo.setPosition(safteyDownPos);
rAServo.setPosition(safteyDownPos);
wristServo.setPosition(wristScorePos);
} else if (tape == ArmPos.APosition.SAFTEYUP) {
lAServo.setPosition(safteyUpPos);
rAServo.setPosition(safteyUpPos);
wristServo.setPosition(wristScorePos);
}
}
public void setHopper(HopperPos.hopperPos hopper) {
if (hopper == HopperPos.hopperPos.UP) {
wristServo.setPosition(wristScorePos);
} else if (hopper == HopperPos.hopperPos.DOWN) {
wristServo.setPosition(startingWristPos);
}
}
public String getTelemetry() {
return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition();
}
public void update() {
armController.setP(0.01);
this.armController.setSetPoint(this.armControllerTarget);
double output = this.armController.calculate(this.lAServo.getPosition());
this.lAServo.setPosition(this.lAServo.getPosition() + output);
this.rAServo.setPosition(this.rAServo.getPosition() + output);
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java Normal file
View File

@ -0,0 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
public class ArmPos {
public enum APosition {
SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP;
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class DoorPos {
public enum DoorPosition {
OPEN, CLOSE
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class Down {
public enum DownArm {
YES, NO
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class Hieght {
public enum height {
ASCEND, HOLD, DESCEND, RESET
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class HopperPos {
public enum hopperPos {
UP, DOWN
}
}

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java
View File

@ -6,12 +6,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@Config
public class Intake { // TODO done in theory, but need to get the actual servo positions &&&& do the Sensor stuff
public class Intake {
private Servo rServo;
private Servo lServo;
private DcMotor dcMotor;
private Position pos = Position.UP;
public enum Position {
STACK1, STACK2, STACK3, STACK4, STACK5, UP;
@ -27,12 +26,12 @@ public class Intake { // TODO done in theory, but need to get the actual servo p
}
//Position
public static double stack1 = 0.49;
public static double stack2 = 0.50;
public static double stack3 = 0.51;
public static double stack4 = 0.52;
public static double stack5 = 0.53;
public static double up = 0.551;
public static double stack1 = 0;
public static double stack2 = 0.03;
public static double stack3 = 0.06;
public static double stack4 = 0.09;
public static double stack5 = 0.12;
public static double up = 0.30;
public static double motorPower = 0;
public Intake(HardwareMap hardwareMap) {

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -8,31 +8,41 @@ import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
@Config
public class Robot {
//set to public Drive to revert
//Create each object
public SampleMecanumDrive drive;
public Camera camera;
public Intake intake;
public Slides slides;
public Arm arm;
private boolean camEnabled = true;
//Name the objects
public Robot(HardwareMap hardwareMap) {
//change this to new Drive to revert
arm = new Arm(hardwareMap);
drive = new SampleMecanumDrive(hardwareMap);
slides = new Slides(hardwareMap);
camera = new Camera(hardwareMap);
camera.initTargetingCamera();
intake = new Intake(hardwareMap);
camEnabled = true;
}
//Update on runtime and dive
public void update(double runTime) {
drive.update();
}
//Return position to control hub
public String getTelemetry() {
Encoder slide = null;
return String.format("position: %s", slide.getCurrentPosition());
}
//set to public Drive to revert
public Arm getArm() {
return this.arm;
}
//return drive
public SampleMecanumDrive getDrive() {
return this.drive;
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java Normal file
View File

@ -0,0 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
public class SlidePos {
public enum SPosition {
DOWN, TAPE1, TAPE2, TAPE3, MAX;
}
}

138
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java Normal file
View File

@ -0,0 +1,138 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
@Config
public class Slides {
//Create and assign motors
public Slides(HardwareMap hardwareMap) {
rSMotor = hardwareMap.get(DcMotor.class, "Right Slide Motor");
// rSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
rSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
lSMotor = hardwareMap.get(DcMotor.class, "Left Slide Motor");
lSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
lSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
lSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
//make each motor
private DcMotor rSMotor;
private DcMotor lSMotor;
//Value for motor power and speed slides go at
public static double rSPower = 0.5;
public static double lSPower = 0.5;
public static double downpos = 0;
public static double tape1pos = 200;
public static double tape2pos = 350;
public static double tape3pos = 500;
public static double maxpos = 1000;
//Set default slide height to 0 aka starting position
public static double sHeight = 0;
public static double newSHeight = 0;
// Do stuff to get to a position when asked to go to a position
public void setSPos(SlidePos.SPosition height) {
if (height == SlidePos.SPosition.DOWN) {
sHeight = 0;
lSMotor.setTargetPosition((int) downpos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) downpos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.TAPE1) {
sHeight = 300;
lSMotor.setTargetPosition((int) tape1pos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) tape1pos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.TAPE2) {
sHeight = 500;
lSMotor.setTargetPosition((int) tape2pos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) tape2pos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.TAPE3) {
sHeight = 700;
lSMotor.setTargetPosition((int) tape3pos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) tape3pos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.MAX) {
sHeight = 1000;
lSMotor.setTargetPosition((int) maxpos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) maxpos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
}
}
// public void setHeight(Hieght.height send) {
// if (send == Hieght.height.ASCEND) {
// if (newSHeight <= 790) {
// newSHeight = newSHeight + 10;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// } else if (newSHeight >= 800) {
// newSHeight = 800;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// }
// } else if (send == Hieght.height.HOLD) {
// newSHeight = newSHeight + 0;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// } else if (send == Hieght.height.DESCEND) {
// if (newSHeight >= 10) {
// newSHeight = newSHeight - 10;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// } else if (newSHeight <= 0) {
// newSHeight = 0;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// }
// }
// }
//Return feed to the control hub for easy debug
public String getTelemetry() {
return "Left Slide Motor: "+lSMotor.getPower()+"Right Slide Motor: "+rSMotor.getPower();
}
}

169
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java
View File

@ -1,29 +1,40 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.util.CameraPosition;
import java.util.ArrayList;
import java.util.Locale;
public abstract class AbstractAuto extends LinearOpMode {
public Robot robot;
private int teamElementLocation = 2;
private double currentRuntime;
private int teamElementLocation = 2;
private ArrayList<Step> steps;
private double currentRuntime;
public CameraPosition cameraPosition;
// Main method to run all the steps for autonomous
@Override
public void runOpMode() {
// telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Initializing Robot...");
telemetry.update();
setCameraPosition();
robot = new Robot(hardwareMap);
makeTrajectories();
while (robot.camera.getFrameCount() < 1) {
idle();
}
// wait for start
while (!(isStarted() || isStopRequested())) {
currentRuntime = getRuntime();
robot.update(currentRuntime);
@ -34,17 +45,161 @@ public abstract class AbstractAuto extends LinearOpMode {
}
telemetry.addLine("Initialized");
telemetry.addLine("Randomization: " + teamElementLocation);
telemetry.addLine("Randomization: "+teamElementLocation);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
resetRuntime();
// build the first step
steps = new ArrayList<>();
stopCamera();
initializeSteps(teamElementLocation);
int stepNumber = 0;
double stepTimeout;
Step step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
// run the remaining steps
while (opModeIsActive()) {
currentRuntime = getRuntime();
// once a step finishes
if (step.isFinished() || currentRuntime >= stepTimeout) {
// do the finishing move
step.end();
stepNumber++;
// if it was the last step break out of the while loop
if (stepNumber > steps.size() - 1) {
break;
}
// else continue to the next step
step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
}
// // update turret and slides position
// PoseStorage.slidesPosition = robot.actuators.getSlides();
// PoseStorage.turretPosition = robot.actuators.getTurret();
// PoseStorage.currentPose = robot.drive.getPoseEstimate();
// while the step is running display telemetry
step.whileRunning();
robot.update(currentRuntime);
telemetry.addLine(String.format(Locale.US, "Runtime: %.0f", currentRuntime));
telemetry.addLine("Step " + (stepNumber + 1) + " of " + steps.size() + ", " + step.getTelemetry() + "\n");
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}
public abstract void setAlliance();
// Load up all of the steps for the autonomous: to be overridden with the specific steps in the specific auto
public abstract void initializeSteps(int location);
public abstract void makeTrajectories();
//methods to be implemented in the specific autos
public abstract void setAlliance();
public abstract void setCameraPosition();
public abstract boolean useCamera();
public abstract void makeTrajectories();
// public abstract void setArm(Arm.Position armPos, Claw.Position clawPos);
//other methods that do certain tasks
public void turn(double degrees) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.turn(degrees);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followTrajectory(Trajectory trajectory) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
// Functions to add steps
public void addDelay(double timeout) {
steps.add(new Step("Waiting for " + timeout + " seconds", timeout) {
@Override
public void start() {
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return false;
}
});
}
public void stopCamera() {
steps.add(new Step("Stopping Signal Camera") {
@Override
public void start() {
robot.camera.stopTargetingCamera();
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return true;
}
});
}
}

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java Normal file
View File

@ -0,0 +1,27 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous
public class BotGoMove extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory myTrajectory = drive.trajectoryBuilder(new Pose2d())
.forward(3.7)
.build();
waitForStart();
if(isStopRequested()) return;
drive.followTrajectory(myTrajectory);
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java
View File

@ -1,11 +1,12 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.hardware.Camera;
import org.firstinspires.ftc.teamcode.hardware.Drive;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@ -14,24 +15,22 @@ import org.firstinspires.ftc.teamcode.util.CameraPosition;
@Autonomous(name = "Start From Left Wall", group = "Left Start", preselectTeleOp = "Khang Main")
public class StartFromLeftCenterSpike extends AbstractAuto {
//set to public Drive to revert
public SampleMecanumDrive drive;
public Robot robot;
public Camera camera;
private boolean camEnabled = true;
public CameraPosition cameraPosition;
private int teamElementLocation = 2;
//Steps
public Trajectory start;
public Trajectory step1;
public Trajectory step2;
@Override
public void initializeSteps(int location) {
followTrajectory(start);
}
@Override
public void setAlliance() {}
@Override
public void setCameraPosition() {
cameraPosition = CameraPosition.CENTER;
cameraPosition = CameraPosition.FRONT;
}
@Override
@ -39,27 +38,22 @@ public class StartFromLeftCenterSpike extends AbstractAuto {
return true;
}
public void Robot(HardwareMap hardwareMap) {
//set to new Drive to revert
drive = new SampleMecanumDrive(hardwareMap);
camera = new Camera(hardwareMap);
camera.initTargetingCamera();
camEnabled = true;
@Override
public void waitForStart() {
super.waitForStart();
}
@Override
public void makeTrajectories() {
// positions
Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(-90));
Pose2d step1 = new Pose2d(-24,-48,Math.toRadians(0));
Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(0));
Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(90));
Vector2d step1 = new Vector2d(-24,-48);
Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(90));
this.start = robot.drive.trajectoryBuilder(start)
.lineToSplineHeading(step1,
SampleMecanumDrive.getVelocityConstraint(60, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
// this.start = robot.drive.trajectoryBuilder(start)
this.start = robot.drive.trajectoryBuilder(new Pose2d())
.forward(3)
.build();
}
}

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java Normal file
View File

@ -0,0 +1,53 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import org.firstinspires.ftc.teamcode.vision.Detection;
// Class for every step that the autonomous program will take
public abstract class Step {
private final double timeout;
private String telemetry;
// variables when moving
public double x;
public double y;
public double stepTime;
double tempTime = stepTime;
// variables when shooting
public Detection teamProp;
// Constructors
public Step(String telemetry) {
this.telemetry = telemetry;
this.timeout = -1;
}
public Step(String telemetry, double timeout) {
this.telemetry = telemetry;
this.timeout = timeout;
}
// Abstract methods to be overrode
public abstract void start();
public abstract void whileRunning();
public abstract void end();
public abstract boolean isFinished();
// Return the timeout for the step
public double getTimeout() {
return timeout;
}
public void setTelemetry(String telemetry) {
this.telemetry = telemetry;
}
// Return the Telemetry for the step
public String getTelemetry() {
return telemetry;
}
}

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/TestAuto.java Normal file
View File

@ -0,0 +1,29 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Config
@Autonomous (name = "Start From Left Center Spike Test", group = "Testing", preselectTeleOp = "KhangMain")
public class TestAuto extends LinearOpMode {
private Robot robot;
@Override
public void runOpMode() throws InterruptedException {
this.robot = new Robot(hardwareMap);
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
//bot is 15 in and a half so middle is 7.75
//17.8 in so half is 8.9 in
Pose2d startPOS = new Pose2d(-63.1, -36.75, Math.toRadians(90));
}
}

206
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java
View File

@ -1,71 +1,207 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Camera;
import org.firstinspires.ftc.teamcode.hardware.ArmPos;
import org.firstinspires.ftc.teamcode.hardware.DoorPos;
import org.firstinspires.ftc.teamcode.hardware.Down;
import org.firstinspires.ftc.teamcode.hardware.Hieght;
import org.firstinspires.ftc.teamcode.hardware.HopperPos;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.SlidePos;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
@TeleOp(name = "Khang Main", group = "OpModes")
@Config
@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
public class KhangMain extends OpMode {
//create and set start position of intake
private Intake.Position Currentpos = Intake.Position.UP;
//inform if slides are in lowest position or not
private Down.DownArm DownQuestion = Down.DownArm.YES;
//create and set start position of slides
private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
//create and set start position of arm
private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN;
//create and set start position of door, default close
private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
//create and set start position of hopper
private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
//create manual slide height varable and set it to hold position as start
private Hieght.height CurrentHeight = Hieght.height.HOLD;
//create robot instance
private Robot robot;
//create servo for plane
private Servo planeServo;
//create controller 1 (driver)
private Controller controller1;
//create controller 2 (macro user and one with hard life)
private Controller controller2;
//set starting slide height to 0
private double sHeight = 0;
@Override
public void init() {
//make each servo, motor, and controller and configure them
planeServo = hardwareMap.get(Servo.class, "Plane Servo");
this.robot = new Robot(hardwareMap);
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
//feedback to driver hub to know if init was successful
telemetry.addLine("Started");
//update to amke sure we receive last lien of code
telemetry.update();
}
@Override
public void loop() {
// this.robot.getDrive().setInput(gamepad1, gamepad2);
double x = gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x;
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//create and set X, Y, and Z for drive inputs
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//set intake to be pressure reactant to right trigger
robot.intake.setDcMotor(gamepad2.right_trigger);
//create variable for door open or close to open and close in sync with intake
double door = gamepad2.right_trigger;
//manual slide height control
double sHeight = gamepad2.right_stick_y;
//set slides all the way down aka reset button
double intakeON = gamepad2.right_trigger;
//graph input of Y joystick to make macros for slides
double sY = -gamepad2.left_stick_y;
//graph of X
double sX = gamepad2.left_stick_x;
//variable to determine shoot drone or not
double shoot = gamepad1.right_trigger;
//update variables to constantly feed position each component should be in
//intake
robot.intake.setpos(Currentpos);
//slide macro
robot.slides.setSPos(CurrentSpos);
//slide manual
// robot.slides.setHeight(CurrentHeight);
//arm position
robot.arm.setPos(CurrentApos);
//door open or not
robot.arm.openDoor(CurrentDpos);
//hopper position
robot.arm.setHopper(hopperpos);
//update
controller2.update();
//manual slide input reader
// if (sHeight >= 0.2) {
// CurrentHeight = Hieght.height.ASCEND;
// } else if (sHeight <= -0.2) {
// CurrentHeight = Hieght.height.DESCEND;
// } else {
// CurrentHeight = Hieght.height.HOLD;
// }
//read values to determine if plane should shoot or not
if (shoot >= 0.9) {
planeServo.setPosition(0.2);
} else {
planeServo.setPosition(0);
}
//make door rise as intake goes on
if (intakeON >= 0.01) {
CurrentDpos = DoorPos.DoorPosition.OPEN;
} else {
CurrentDpos = DoorPos.DoorPosition.CLOSE;
}
//control position of hopper
if (controller2.getLeftBumper().isJustPressed()) {
Currentpos = Currentpos.nextPosition();
hopperpos = HopperPos.hopperPos.UP;
} else if (controller2.getLeftBumper().isJustReleased()) {
hopperpos = HopperPos.hopperPos.DOWN;
}
if (controller2.getRightBumper().isJustPressed()) {
Currentpos = Currentpos.previousPosition();
//shift intake up one pixel
if (controller2.getDLeft().isJustPressed()) {
//prevent from going higher than highest legal value
if (Currentpos != Intake.Position.UP) {
Currentpos = Currentpos.nextPosition();
}
}
//shift intake down one pixel
if (controller2.getDRight().isJustPressed()) {
//prevent from going lower than lowest value
if (Currentpos != Intake.Position.STACK1) {
Currentpos = Currentpos.previousPosition();
}
}
//set intake to max position
if (controller2.getDUp().isJustPressed()) {
Currentpos = Currentpos.UP;
}
// Read pose
// Pose2d poseEstimate = drive.getPoseEstimate();
//
//// Create a vector from the gamepad x/y inputs
//// Then, rotate that vector by the inverse of that heading
// Vector2d input = new Vector2d(
// -gamepad1.left_stick_y,
// -gamepad1.left_stick_x
// ).rotated(-poseEstimate.getHeading());
//
//// Pass in the rotated input + right stick value for rotation
//// Rotation is not part of the rotated input thus must be passed in separately
// drive.setWeightedDrivePower(
// new Pose2d(
// input.getX(),
// input.getY(),
// -gamepad1.right_stick_x
// )
// );
//set intake to lowest position
if (controller2.getDDown().isJustPressed()) {
Currentpos = Currentpos.STACK1;
}
//arm safety pause going up
if (controller2.getA().isJustPressed()){
CurrentApos = CurrentApos.SAFTEYUP;
DownQuestion = DownQuestion.NO;
}
//arm safety pause going down
if (controller2.getX().isJustPressed()) {
CurrentApos = CurrentApos.SAFTEYDOWN;
DownQuestion = DownQuestion.NO;
}
//arm all the way up
if (controller2.getB().isJustPressed()) {
CurrentApos = CurrentApos.SCORE;
DownQuestion = DownQuestion.NO;
}
//arm all the way down
if (controller2.getY().isJustPressed()) {
CurrentApos = CurrentApos.SDOWN;
DownQuestion = DownQuestion.YES;
}
if (sY >= 0.2) {
CurrentSpos = CurrentSpos.MAX;
}
if (sY <= -0.2) {
CurrentSpos = CurrentSpos.TAPE1;
}
if (sX >= 0.2) {
CurrentSpos = CurrentSpos.TAPE3;
}
if (sX <= -0.2) {
CurrentSpos = CurrentSpos.TAPE2;
}
if (gamepad2.left_trigger >= 0.35) {
CurrentSpos = CurrentSpos.DOWN;
}
// if (gamepad2.left_stick_button = true) {
// CurrentSpos = CurrentSpos.DOWN;
// }
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java
View File

@ -31,7 +31,7 @@ public class DriveConstants {
* If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = true;
public static final boolean RUN_USING_ENCODER = false;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java
View File

@ -136,7 +136,7 @@ public class SampleMecanumDrive extends MecanumDrive {
// TODO: if desired, use setLocalizer() to change the localization method
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
// setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
// setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap));
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
@ -282,8 +282,8 @@ public class SampleMecanumDrive extends MecanumDrive {
public void setMotorPowers(double v, double v1, double v2, double v3) {
leftFront.setPower(v);
leftRear.setPower(v1);
rightRear.setPower(v2);
rightFront.setPower(v3);
rightRear.setPower(-v2);
rightFront.setPower(-v3);
}
@Override
@ -315,12 +315,4 @@ public class SampleMecanumDrive extends MecanumDrive {
public String getTelemetry() {
return "Drive: "+getPoseEstimate();
}
public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double x = gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x;
setInput(gamepad1, gamepad2);
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -61,13 +61,13 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
this.drive = drive;
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel"));
// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel"));
// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular"));
// perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);
parallelEncoder.setDirection(Encoder.Direction.REVERSE);
// perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);
// parallelEncoder.setDirection(Encoder.Direction.REVERSE);
}
public static double encoderTicksToInches(double ticks) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java
View File

@ -1,5 +1,5 @@
package org.firstinspires.ftc.teamcode.util;
public enum CameraPosition {
LEFT, CENTER, RIGHT
FRONT, BACK
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.util;
import androidx.annotation.VisibleForTesting;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.opencv.core.Mat;
import org.opencv.core.Point;
@ -51,4 +53,6 @@ public class Constants {
public static final String STACK_WEBCAM = "Stack Webcam";
public static final String TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu";
public static final String lServo = "lServo";
public static final String rServo = "rServo";
}

1
build.common.gradle
View File

@ -121,5 +121,6 @@ android {
}
repositories {
mavenCentral()
}