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

tweaks all around, deleted a lot of unused files, started a cycle auto

This commit is contained in:
Robert Teal 2023-12-19 23:19:07 -06:00
parent 9f8bd3dbaa
commit 4625c3f784
27 changed files with 283 additions and 1121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -5,79 +5,102 @@ import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
@Config
public class Arm {
//make each servo
private Servo dServo;
private Servo rAServo;
private Servo lAServo;
private Servo doorServo;
private Servo wristServo;
private Slides.Position pos = Slides.Position.DOWN;
private PController armController;
private double armControllerTarget;
public static double ARM_KP = 0.01;
public static double armTolerance = 0.03;
public static double armSpeed = 0.022;
private double armPos;
private double armTempPos;
private double doorPos;
private double wristPos;
public static double ARM_KP = 0.08;
public static double doorOpenPos = 0.09;
public static double doorClosePos = 0.26;
public static double armStart = 0.24;
public static double armScore = 0.95;
public static double wristStart = 0.29;
public static double wristScore = 0.5;
public enum Position {
INTAKE, SCORE;
INTAKE, SCORE
}
public enum DoorPosition {
OPEN, CLOSE
}
public Arm(HardwareMap hardwareMap) {
wristServo = hardwareMap.get(Servo.class, "Wrist Servo");
dServo = hardwareMap.get(Servo.class, "Door Servo");
doorServo = 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);
doorServo.setDirection(Servo.Direction.REVERSE);
// wristServo.setDirection(Servo.Direction.REVERSE);
this.armController = new PController(ARM_KP);
setArmPos(Position.INTAKE);
lAServo.setPosition(armStart);
rAServo.setPosition(armStart);
// armTempPos = armPos;
setWristPos(Position.INTAKE);
setDoor(DoorPosition.CLOSE);
}
public static double doorOpenpos = 0.1;
public static double doorClosePos = 0.85;
public static double armStart = 0.15;
public static double armScore = 1;
public static double wristStart = 0.93;
public static double wristScore = 0.98;
public void setArmPos(Position pos) {
this.armControllerTarget = pos == Position.INTAKE
? armStart
: armScore;
// if (pos == Position.INTAKE) {
// armPos = armStart;
// } else if (pos == Position.SCORE) {
// armPos = armScore;
// }
}
public boolean armAtPosition() {
return armController.atSetPoint();
// return armTempPos == armPos;
}
public void setWristPos(Position pos) {
if (pos == Position.INTAKE) {
wristServo.setPosition(wristStart);
wristPos = wristStart;
} else if (pos == Position.SCORE) {
wristServo.setPosition(wristScore);
wristPos = wristScore;
}
}
public void setDoor(DoorPos.DoorPosition pos) {
if (pos == DoorPos.DoorPosition.OPEN) {
dServo.setPosition(doorOpenpos);
} else if (pos == DoorPos.DoorPosition.CLOSE) {
dServo.setPosition(doorClosePos);
public void setDoor(DoorPosition pos) {
if (pos == DoorPosition.OPEN) {
doorPos = doorOpenPos;
} else if (pos == DoorPosition.CLOSE) {
doorPos = doorClosePos;
}
}
public String getTelemetry() {
return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition();
return String.format("Wrist: %s\nDoor: %s\nLeft Arm: %s\nRight Arm: %s\nError: %s",
wristServo.getPosition(), doorServo.getPosition(), lAServo.getPosition(), rAServo.getPosition(), armController.getPositionError());
//return "Wrist Servo: "+wristServo.getPosition()+"\nLeft Arm Servo: "+lAServo.getPosition()+"\nRight Arm Servo: "+rAServo.getPosition()+"\nDoor Servo: "+ doorServo.getPosition();
}
public void update() {
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setTolerance(0.001);
this.armController.setTolerance(armTolerance);
this.armController.setP(ARM_KP);
double output = 0;
@ -86,6 +109,23 @@ public class Arm {
this.lAServo.setPosition(this.lAServo.getPosition() + output);
this.rAServo.setPosition(this.lAServo.getPosition() + output);
} else {
lAServo.setPosition(armControllerTarget);
rAServo.setPosition(armControllerTarget);
}
// if (Math.abs(armTempPos-armPos) <= armTolerance) {
// armTempPos = armPos;
// } else {
// if (armTempPos < armPos) {
// armTempPos += armSpeed;
// } else if (armTempPos > armPos) {
// armTempPos -= armSpeed;
// }
// }
// lAServo.setPosition(armTempPos);
// rAServo.setPosition(armTempPos);
doorServo.setPosition(doorPos);
wristServo.setPosition(wristPos);
}
}

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

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

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

@ -17,20 +17,22 @@ import org.openftc.apriltag.AprilTagDetection;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import com.acmerobotics.dashboard.FtcDashboard;
import java.util.ArrayList;
// Class for the camera
public class Camera {
private HardwareMap hardwareMap;
private OpenCvCamera targetingCamera;
private TargetingPipeline targetingPipeline;
private boolean targetingCameraInitialized;
private float decimation;
private boolean needToSetDecimation;
int numFramesWithoutDetection = 0;
private boolean signalWebcamInitialized;
private OpenCvCamera signalWebcam;
private HardwareMap hardwareMap;
private OpenCvCamera targetingCamera;
private TargetingPipeline targetingPipeline;
private boolean targetingCameraInitialized;
AprilTagDetectionPipeline aprilTagDetectionPipeline;
ArrayList<AprilTagDetection> detections;
static final double FEET_PER_METER = 3.28084;
@ -57,6 +59,7 @@ public class Camera {
public void onOpened() {
targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION);
targetingCameraInitialized = true;
FtcDashboard.getInstance().startCameraStream(signalWebcam, 0);
}
@Override
@ -85,8 +88,8 @@ public class Camera {
//return frame rate
public int getFrameCount() {
if (signalWebcamInitialized) {
return signalWebcam.getFrameCount();
if (targetingCameraInitialized) {
return targetingCamera.getFrameCount();
} else {
return 0;
}

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

@ -1,7 +0,0 @@
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
View File

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

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java
View File

@ -11,7 +11,7 @@ public class DroneLauncher {
public static double launchPos = 0.5;
public DroneLauncher(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, Constants.droneLauncher);
this.servo = hardwareMap.get(Servo.class, "Drone Launcher");
this.servo.setPosition(initPos);
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java
View File

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

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

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

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

@ -1,5 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -36,9 +39,9 @@ public class Intake {
public static double motorPower = 0;
public Intake(HardwareMap hardwareMap, Position up) {
lServo = hardwareMap.get(Servo.class, "Left Servo");
lServo = hardwareMap.get(Servo.class, "Left Intake Servo");
lServo.setDirection(Servo.Direction.REVERSE);
rServo = hardwareMap.get(Servo.class, "Right Servo");
rServo = hardwareMap.get(Servo.class, "Right Intake Servo");
dcMotor = hardwareMap.get(DcMotor.class, "Intake Motor");
}

138
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java
View File

@ -1,138 +0,0 @@
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 KhangSlides {
//Create and assign motors
public KhangSlides(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();
}
}

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

@ -1,15 +1,13 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
@Config
public class Robot {
@ -21,6 +19,7 @@ public class Robot {
public Slides slides;
public Arm arm;
public DroneLauncher droneLauncher;
public double macroStartTime = 0;
public int macroState = 0;
public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return
@ -28,6 +27,10 @@ public class Robot {
private boolean camEnabled = true;
public static double slideWait = 1.5;
public static double scoreWait = 1.25;
public static double armWait = 2.0;
//Name the objects
public Robot(HardwareMap hardwareMap) {
drive = new SampleMecanumDrive(hardwareMap);
@ -49,12 +52,11 @@ public class Robot {
macroState ++;
break;
case(1):
if (runTime > macroStartTime + 1) {
if (runTime > macroStartTime + slideWait || slides.atTarget()) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState = 0;
@ -71,9 +73,8 @@ public class Robot {
arm.setDoor(OPEN);
macroState++;
break;
//Ind_sleeper
case(1):
if (runTime > macroStartTime + 1) {
if (runTime > macroStartTime + scoreWait) {
macroState ++;
}
break;
@ -84,12 +85,11 @@ public class Robot {
macroState++;
break;
case(3):
if (runTime > macroStartTime + 1.1) {
if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
macroState ++;
}
break;
case(4):
macroStartTime = runTime;
slides.setTarget(pos);
macroState = 0;
lastMacro = runningMacro;
@ -104,6 +104,6 @@ public class Robot {
}
public String getTelemetry() {
return "Telemetry?";
return arm.getTelemetry();
}
}

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

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

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.hardware.robby;
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PIDController;
@ -19,12 +19,12 @@ public class Slides {
public static PIDController controller = new PIDController(p, i, d);
public static int targetMin = -10;
public static int targetMax = 1000;
public static int targetMax = 555;
public static int down = 0;
public static int tier1 = 200;
public static int tier2 = 350;
public static int tier3 = 500;
public static int tier1 = 350;
public static int tier2 = 450;
public static int tier3 = 550;
private int target = 0;
@ -36,13 +36,13 @@ public class Slides {
slide = hardwareMap.get(DcMotor.class, "Right Slide Motor");
slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// slide.setDirection(DcMotorSimple.Direction.REVERSE);
slide.setDirection(DcMotorSimple.Direction.REVERSE);
slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
slide2 = hardwareMap.get(DcMotor.class, "Left Slide Motor");
slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
slide2.setDirection(DcMotorSimple.Direction.REVERSE);
// slide2.setDirection(DcMotorSimple.Direction.REVERSE);
slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PoseStorage.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
package org.firstinspires.ftc.teamcode.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;

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

@ -3,8 +3,10 @@ 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.Arm;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.util.CameraPosition;
import java.util.ArrayList;
@ -23,7 +25,7 @@ public abstract class AbstractAuto extends LinearOpMode {
public void runOpMode() {
// telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Initializing Robot...");
telemetry.addLine("Initializing...");
telemetry.update();
setCameraPosition();
@ -32,9 +34,9 @@ public abstract class AbstractAuto extends LinearOpMode {
makeTrajectories();
// while (robot.camera.getFrameCount() < 1) {
// idle();
// }
while (robot.camera.getFrameCount() < 1) {
idle();
}
// wait for start
while (!(isStarted() || isStopRequested())) {
@ -222,6 +224,54 @@ public abstract class AbstractAuto extends LinearOpMode {
});
}
public void intakeStack(Intake.Position stackHeight1, Intake.Position stackHeight2) {
steps.add(new Step("Intaking Stack") {
@Override
public void start() {
robot.intake.setDcMotor(0.5);
robot.arm.setDoor(Arm.DoorPosition.OPEN);
}
@Override
public void whileRunning() {
switch(macroState) {
case 0:
macroStart = currentRuntime;
robot.intake.setpos(stackHeight1);
macroState++;
break;
case 1:
if (currentRuntime > macroStart + 1.0) {
macroState++;
}
break;
case 2:
robot.intake.setpos(stackHeight2);
macroState++;
break;
case 3:
if (currentRuntime > macroStart + 1.0) {
macroState++;
}
break;
case 4:
macroState = 0;
}
}
@Override
public void end() {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
}
@Override
public boolean isFinished() {
return macroState == 0;
}
});
}
public void runIntake(double power, double timeout) {
steps.add(new Step("Running Intake" + timeout + " seconds", timeout) {
@Override

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java
View File

@ -4,7 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.hardware.Slides;
@Autonomous(name = "Blue Left Auto")
public class BlueLeftAuto extends AbstractAuto {

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

@ -0,0 +1,98 @@
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 org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "Red Right Auto")
public class RedRightAuto extends AbstractAuto {
public Trajectory scorePurple;
public Trajectory scoreYellow;
public Trajectory load1;
public Trajectory score1;
public Trajectory parkRobot;
@Override
public void makeTrajectories() {
Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
Pose2d dropMiddle = new Pose2d(12, -40.5, Math.toRadians(90));
Pose2d depositPreload = new Pose2d(46, -36, Math.toRadians(180));
Pose2d lineup1 = new Pose2d(12, -14, Math.toRadians(180));
Pose2d pickup1 = new Pose2d(-58, -14, Math.toRadians(180));
Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180));
Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180));
Pose2d park = new Pose2d(48, -12, Math.toRadians(180));
scorePurple = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(depositPreload)
.build();
load1 = robot.drive.trajectoryBuilder(scoreYellow.end())
.splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.lineToLinearHeading(pickup1,
SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
score1 = robot.drive.trajectoryBuilder(load1.end(), true)
.lineToLinearHeading(driveup1,
SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
parkRobot = robot.drive.trajectoryBuilder(score1.end())
.lineToLinearHeading(park)
.build();
robot.drive.setPoseEstimate(start);
}
@Override
public void initializeSteps(int location) {
followTrajectory(scorePurple);
runIntake(-0.4, 0.5);
// followAndExtend(scoreYellow, Slides.Position.TIER1);
// followAndRetract(load1, Slides.Position.DOWN);
followTrajectory(scoreYellow);
followTrajectory(load1);
intakeStack(Intake.Position.STACK5, Intake.Position.STACK4);
followAndExtend(score1, Slides.Position.TIER1);
followAndRetract(parkRobot, Slides.Position.DOWN);
}
@Override
public void setAlliance() {
}
@Override
public void setCameraPosition() {
}
@Override
public boolean useCamera() {
return false;
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java
View File

@ -8,14 +8,9 @@ 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;
// macro variables
public double macroStart;
public int macroState;
// Constructors
public Step(String telemetry) {

304
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java
View File

@ -1,304 +0,0 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//
//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.BLUE;
//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.RED;
//import static java.lang.Math.PI;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.arcrobotics.ftclib.controller.PIDController;
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//
//import org.firstinspires.ftc.teamcode.controller.Controller;
//import org.firstinspires.ftc.teamcode.hardware.Arm;
//import org.firstinspires.ftc.teamcode.hardware.Robot;
//import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
//
//@Config
//public abstract class AbstractTeleOp extends OpMode {
// private Robot robot;
// public Team team;
// Controller driver1;
// Controller driver2;
//
// public static double normal = 0.5;
// public static double vazrooooming = 1.0;
// public static int heightIncrement = 20;
//
// Pose2d robot_pos;
// double robot_x, robot_y, robot_heading;
//
// // auto align variables
// double headingPID;
// public static double headingP = 0.01;
// public static double headingI = 0.03;
// public static double headingD = 0.0005;
// public static PIDController headingController = new PIDController(headingP, headingI, headingD);
//
// double strafePID;
// public static double strafeP = 0.05;
// public static double strafeI = 0;
// public static double strafeD = 0.01;
// public static PIDController strafeController = new PIDController(strafeP, strafeI, strafeD);
//
// double robot_y_pos;
// boolean fixed90Toggle = false;
// boolean fixed0Toggle = false;
//
//// public static double robot_width = 12;
//// public static double robot_length = 12;
//// public static double robot_radius = 6;
////
//// public static double groundJuncRadius = 6;
//// public static double coneRadius = 4;
////
//// public static double armWait = 0.2;
//
//// private double timeSinceOpened = 0; // for claw
//// private double timeSinceClosed = 0;
//
// // private int delayState = 0; // for arm
//// private double delayStart = 0;
//// private boolean doArmDelay = false; // for arm
// private boolean isAutoClose = true;
//
// @Override
// public void init() {
// robot = new Robot(hardwareMap);
// driver1 = new Controller(gamepad1);
// driver2 = new Controller(gamepad2);
// if (team == RED) {
// driver1.setColor(255, 0, 0);
// driver2.setColor(255, 0, 0);
// } else if (team == BLUE) {
// driver1.setColor(0, 0, 255);
// driver2.setColor(0, 0, 255);
// }
//
// PoseStorage.AutoJustEnded = false;
// }
//
// @Override
// public void init_loop() {
// ////init when camera is on and responding
//// if (robot.camera.getFrameCount() < 1) {
//// telemetry.addLine("Initializing Robot...");
//// } else {
//// telemetry.addLine("Initialized");
//// }
// telemetry.addLine("Initialized");
// telemetry.addLine(robot.getTelemetry());
// telemetry.update();
// }
//
// //find out what quarter you are in
// private int getQuadrant(double angle) {
// if (0 < angle && angle < PI/2.0) {
// return 1;
// } else if (PI/2.0 < angle && angle < PI) {
// return 2;
// } else if (PI < angle && angle < 3*PI/2.0) {
// return 3;
// } else {
// return 4;
// }
// }
//
// @Override
// public void loop() {
// // robot position update
// robot_pos = robot.drive.getPoseEstimate();
// robot_x = robot_pos.getX();
// robot_y = robot_pos.getY();
// robot_heading = robot_pos.getHeading(); // in radians
//
// // driver 1 controls
// driver1.update();
// driver2.update();
//
// //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;
//
//// figure out if a toggle is present
// if (driver1.getRightBumper().isPressed()) {
// if (!fixed90Toggle) {
// robot_y_pos = robot.drive.getPoseEstimate().getX();
// }
// fixed90Toggle = true;
// } else {
// fixed90Toggle = false;
// }
//
// if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) {
// if (!fixed0Toggle) {
// robot_y_pos = robot.drive.getPoseEstimate().getY();
// }
// fixed0Toggle = true;
// } else {
// fixed0Toggle = false;
// }
//
// // heading pid
// headingController.setPID(headingP, headingI, headingD);
// int quadrant = getQuadrant(robot_heading);
// if (fixed0Toggle) {
// switch (quadrant) {
// case 1:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 0);//- right
// break;
// case 2:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//+ left
// break;
// case 3:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//- right
// break;
// case 4:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 360);//+ left
// break;
// }
// } else if (fixed90Toggle) {
// switch (quadrant) {
// case 1:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//- right
// break;
// case 2:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//+ left
// break;
// case 3:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//- right
// break;
// case 4:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//+ left
// break;
// }
// } else {
// headingPID = 0;
// }
//
// // strafe pid
// strafeController.setPID(strafeP, strafeI, strafeD);
// if (fixed0Toggle) {
// strafePID = strafeController.calculate(robot_y, robot_y_pos);
// } else if (fixed90Toggle) {
// strafePID = strafeController.calculate(robot_x, robot_y_pos);
// } else {
// strafePID = 0;
// }
//
// telemetry.addLine("Wanted Position: "+robot_y_pos);
// telemetry.addLine("Actual Position: "+robot_y);
// telemetry.addLine("PID: "+strafePID);
//
// // turbo
// if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) {
// x *= vazrooooming;
// y *= vazrooooming;
// z *= vazrooooming;
// } else {
// x *= normal;
// y *= normal;
// z *= normal;
// }
//
// // actually set power
// if (fixed0Toggle || fixed90Toggle) {
// robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID));
// } else {
// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// }
//
// // driver 2 controls
// // increment heights
// if (driver2.getDUp().isJustPressed()) {
// Slides.heightOffset += heightIncrement;
// robot.slides.setTarget(robot.slides.getTarget() + heightIncrement);
// } else if (driver2.getDDown().isJustPressed()) {
// Slides.heightOffset -= heightIncrement;
// robot.slides.setTarget(robot.slides.getTarget() - heightIncrement);
// }
//
// switch (robot.runningMacro) {
// case(0): // manual mode
//
// //track pad is rad
// // left right dpad is turn wrist
// robot.slides.increaseTarget(driver2.getLeftStick().getY());
//// if (Math.abs(driver2.getRightStick().getY()) > 0.05) { // close claw if anything is moved
//// robot.claw.close();
//// }
//
//// if (driver2.getRightBumper().isJustPressed()) {
//// robot.arm.goToScore();
//// }
//// if (driver2.getLeftBumper().isJustPressed()) {
//// robot.arm.goToIntake();
//// }
//
// // retract all the time
// if (driver2.getLeftStickButton().isJustPressed()) {
// robot.runningMacro = 5;
// }
//
// // to cancel macro and allow manual movement
//// if (driver2.getLeftStick().getY() > 0.05 || driver2.getRightStick().getY() > 0.05) {
//// robot.runningMacro = 0;
//// robot.lastMacro = 0;
//// }
//
// if (driver2.getTouchpad().isJustPressed()) {
// robot.runningMacro = 4;
// } else if (driver2.getX().isJustPressed()) { // high position [closed, bring up, bring out]
// robot.runningMacro = 3;
// } else if (driver2.getY().isJustPressed()) { // middle position [middle goal level]
// robot.runningMacro = 2;
// } else if (driver2.getB().isJustPressed() && !driver2.getStart().isJustPressed()) { // low position [low goal level]
// robot.runningMacro = 1;
// } else if (driver2.getA().isJustPressed()) {
// if (robot.lastMacro == 0 || robot.lastMacro == 4) { // if not running any macros or picking up cones
// } else { // otherwise, I need to undo a macro
// robot.runningMacro = 5;
// }
// } else {
// if (driver2.getRightBumper().isJustPressed()) {
// isAutoClose = false;
// }
// if (driver2.getLeftBumper().isJustPressed()) {
// }
// }
// break;
// case(1):
//// robot.extendMacro(Slides.Position.LOW, Arm.APosition.SCORE, getRuntime());
// break;
// case(2):
//// robot.extendMacro(Slides.Position.MEDIUM, Arm.APosition.SCORE, getRuntime());
// break;
// case(3):
//// robot.extendMacro(Slides.Position.HIGH, Arm.APosition.SCORE, getRuntime());
// break;
// case(4):
//// robot.extendMacro(Slides.Position.PICKUP, Arm.APosition.SDOWN, getRuntime());
// break;
// case (5): // macro reset
// robot.resetMacroNoDunk(0, getRuntime());
// driver1.rumble(100);
// break;
// }
//
// // cancel the macros
// if (driver2.getRightStickButton().isJustPressed()) {
// robot.runningMacro = 0;
// robot.lastMacro = 0;
// robot.macroState = 0;
// robot.slides.cancel();
// }
//
// // update and telemetry
// robot.update(getRuntime());
//
// telemetry.addLine(robot.getTelemetry());
// telemetry.addLine(String.format("Last Macro: %s\nRunning Macro: %s\nMacroState: %s", robot.lastMacro, robot.runningMacro, robot.macroState));
// telemetry.update();
// }
//}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java
View File

@ -1,11 +0,0 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//
//@TeleOp(name = "Opmode For Team Blue", group = "Competition")
//public class BlueTeam extends AbstractTeleOp {
//
// public BlueTeam() {
// team = Team.BLUE;
// }
//}

257
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java
View File

@ -1,257 +0,0 @@
//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 com.qualcomm.robotcore.util.ElapsedTime;
//
//import org.firstinspires.ftc.teamcode.controller.Controller;
//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;
//
//@Config
//@TeleOp(name = "experimental opmode", group = "OpModes")
//public class ExperimentalKhangMain extends OpMode {
//
// //keep track of runtime for advanced macros
// private ElapsedTime runTime = new ElapsedTime();
//
// //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;
//
// //robot position stuff
// Pose2d robot_pos;
// double robot_x, robot_y, robot_heading;
//
// //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() {
//
// //reset runtime when opmode is initialized
// runTime.reset();
//
// //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() {
//
// // robot position update
// robot_pos = robot.drive.getPoseEstimate();
// robot_x = robot_pos.getX();
// robot_y = robot_pos.getY();
// robot_heading = robot_pos.getHeading(); // in radians
//
// // Calculate the runtime in seconds
// double currentTime = runTime.seconds();
//
// //set start of macro runtime
// double macroStartTime = 0;
//
// //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);
// //manual slide height control
// double sHeight = gamepad2.right_stick_y;
// //activate intake or not
// 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;
//
// //reset value to set slides to starting value
// double reset = gamepad2.left_trigger;
//
// //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);
// //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;
// }
//
// if (intakeON >= 0.35) {
// Currentpos = Intake.Position.STACK1;
// } else {
// Currentpos = Intake.Position.UP;
// }
//
// //reset slide position
// if (reset >= 0.2) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
//// //control position of hopper
//// if (controller2.getLeftBumper().isJustPressed()) {
//// hopperpos = HopperPos.hopperPos.UP;
//// } else if (controller2.getLeftBumper().isJustReleased()) {
//// hopperpos = HopperPos.hopperPos.DOWN;
//// }
//
// //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;
// }
//
// //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;
//// }
//
// if (controller2.getA().isJustPressed()) {
// macroStartTime = currentTime;
// CurrentSpos = CurrentSpos.TAPE1;
// if (macroStartTime + 1000 <= currentTime) {
// CurrentApos = CurrentApos.SCORE;
// }
// }
//
// //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;
// }
//
// //set slides to max when left joystick is up
// if (sY >= 0.5) {
// CurrentSpos = CurrentSpos.MAX;
// }
//
// //set slides to tape 1 level when left joystick is down
// if (sY <= -0.5) {
// CurrentSpos = CurrentSpos.TAPE1;
// }
//
// //set slides to tape 3 when left joystick is right
// if (sX >= 0.2) {
// CurrentSpos = CurrentSpos.TAPE3;
// }
//
// //set slides to tape 2 when left joystick is left
// if (sX <= -0.2) {
// CurrentSpos = CurrentSpos.TAPE2;
// }
//
// //set slides all the way down when left bumper gets pressed
// if (controller2.getLeftBumper().isJustPressed()) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
// //set intake all teh way up when right bumper is pressed
// if (controller2.getRightBumper().isJustPressed()) {
// Currentpos = Currentpos.UP;
// }
//
// // update the runtime on the telemetry
// telemetry.addData("Runtime", currentTime);
// telemetry.update();
// }
//}

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -1,7 +1,7 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
@ -11,14 +11,15 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.hardware.Slides;
@Config
@TeleOp(name = "Main TeleOp", group = "OpModes")
public class NewTeleop extends OpMode {
public class MainTeleOp extends OpMode {
public static double normal = 0.5;
public static double turbo = 1;
public static double intakeMax = 0.65;
private Robot robot;
private Controller controller1;
@ -30,9 +31,14 @@ public class NewTeleop extends OpMode {
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
robot.intake.setpos(Intake.Position.STACK1);
// robot.intake.setpos(Intake.Position.STACK1);
telemetry.addLine("Started");
while (robot.camera.getFrameCount() < 1) {
telemetry.addLine("Initializing...");
telemetry.update();
}
telemetry.addLine("Initialized");
telemetry.update();
}
@ -55,7 +61,7 @@ public class NewTeleop extends OpMode {
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Driver 2
robot.intake.setDcMotor(gamepad2.right_trigger);
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
@ -64,7 +70,7 @@ public class NewTeleop extends OpMode {
}
// Drone launcher
if (controller1.getA().isPressed()) {
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.droneLauncher.launch();
} else {
this.robot.droneLauncher.reset();
@ -74,20 +80,21 @@ public class NewTeleop extends OpMode {
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed()) {
robot.runningMacro = 4;
}
if (robot.intake.getPower() >= 0.01) {
robot.arm.setDoor(OPEN);
} else {
robot.arm.setDoor(CLOSE);
}
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 4;
}
break;
case (1):
robot.extendMacro(Slides.Position.TIER1, getRuntime());

260
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java
View File

@ -1,260 +0,0 @@
//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.Arm;
//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.hardware.SlidePos;
//import com.qualcomm.robotcore.util.ElapsedTime;
//
//@Config
//@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
//public class Meet1TeleOp extends OpMode {
//
// //turbo mode
// public static double normal = 0.5;
// public static double turbo = 1;
//
// //keep track of runtime for advanced macros
// private ElapsedTime runTime = new ElapsedTime();
//
// //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 Arm.Position CurrentApos = Arm.Position.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() {
//
// //reset runtime when opmode is initialized
// runTime.reset();
//
// //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 make sure we receive last line of code
// telemetry.update();
// }
//
// @Override
// public void loop() {
//
// // Calculate the runtime in seconds
// double currentTime = runTime.seconds();
//
// //set start of macro runtime
// double macroStartTime = 0;
//
// //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));
//
// //turbo activation
// if (controller1.getA().isJustPressed()){
// x *= turbo;
// y *= turbo;
// z *= turbo;
// } else if (controller1.getA().isJustReleased()){
// x *= normal;
// y *= normal;
// z *= normal;
// }
//
// //set intake to be pressure reactant to right trigger
// robot.intake.setDcMotor(gamepad2.right_trigger);
// //manual slide height control
// double sHeight = gamepad2.right_stick_y;
// //activate intake or not
// 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;
// double manualSY = gamepad2.right_stick_y;
//
// //reset value to set slides to starting value
// double reset = gamepad2.left_trigger;
//
// //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.setArmPos(CurrentApos);
// //door open or not
// robot.arm.openDoor(CurrentDpos);
// //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;
// }
//
// //make intake go to stack 1 when pressed down
// if (intakeON >= 0.25) {
// Currentpos = Intake.Position.STACK1;
// } else {
// Currentpos = Intake.Position.UP;
// }
//
// //reset slide position
// if (reset >= 0.2) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
//// //control position of hopper
//// if (controller2.getLeftBumper().isJustPressed()) {
//// hopperpos = HopperPos.hopperPos.UP;
//// } else if (controller2.getLeftBumper().isJustReleased()) {
//// hopperpos = HopperPos.hopperPos.DOWN;
//// }
//
// //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;
// }
//
// //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;
// }
//
// //set slides to max when left joystick is up
// if (sY >= 0.5) {
// CurrentSpos = CurrentSpos.MAX;
// }
//
// //set slides to tape 1 level when left joystick is down
// if (sY <= -0.5) {
// CurrentSpos = CurrentSpos.TAPE1;
// }
//
// //set slides to tape 3 when left joystick is right
// if (sX >= 0.2) {
// CurrentSpos = CurrentSpos.TAPE3;
// }
//
// //set slides to tape 2 when left joystick is left
// if (sX <= -0.2) {
// CurrentSpos = CurrentSpos.TAPE2;
// }
//
// if (manualSY >=0.1) {
// CurrentSpos = CurrentSpos.MANUAL;
// }
//
//// //set slides all the way down when left bumper gets pressed
//// if (controller2.getLeftBumper().isJustPressed()) {
//// CurrentSpos = CurrentSpos.DOWN;
//// }
////
//// //set intake all teh way up when right bumper is pressed
//// if (controller2.getRightBumper().isJustPressed()) {
//// Currentpos = Currentpos.UP;
//// }
//
// // update the runtime on the telemetry
// telemetry.addData("Runtime", currentTime);
// telemetry.update();
// }
//}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java
View File

@ -1,11 +0,0 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//
//@TeleOp(name = "Opmode For Team Red", group = "Competition")
//public class RedTeam extends AbstractTeleOp {
//
// public RedTeam() {
// team = Team.RED;
// }
//}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.java
View File

@ -1,6 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
public enum Team {
RED, BLUE
}

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

@ -66,14 +66,11 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
// old is 35 35 120 120
// new is 70 45 90 90
public static double MAX_VEL = 60;
public static double MAX_ACCEL = 60;
public static double MAX_VEL = 50;
public static double MAX_ACCEL = 50;
public static double MAX_ANG_VEL = Math.toRadians(120);
public static double MAX_ANG_ACCEL = Math.toRadians(120);
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}

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

@ -63,7 +63,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backLeft"));
// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "Intake Motor"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);