Skip to content

Commit

Permalink
Testing 2+1
Browse files Browse the repository at this point in the history
  • Loading branch information
@BudsterGaming
BudsterGaming committed Dec 30, 2023
1 parent 2041b81 commit 631fe1d
Show file tree
Hide file tree
Showing 4 changed files with 67 additions and 30 deletions.
90 changes: 62 additions & 28 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@
/**
* ENIGMA Autonomous Example for only vision detection using openCv and park
*/
@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Mutation")
@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Evolution")
public class EnigmaAuto extends LinearOpMode {

public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name
Expand All @@ -90,13 +90,19 @@ public class EnigmaAuto extends LinearOpMode {
double LiftLeftOffset = .04;
double LiftHeight;

private static final double ELBOW_DRIVE= Mutation.ELBOW_DRIVE;
private static final double ELBOW_INTAKE = Mutation.ELBOW_INTAKE;
private static final double WRIST_INTAKE = Mutation.WRIST_INTAKE;
private static final double ELBOW_DRIVE= Evolution.ELBOW_DRIVE;
private static final double ELBOW_INTAKE = Evolution.ELBOW_INTAKE;
private static final double WRIST_INTAKE = Evolution.WRIST_INTAKE;
//private static final double SHOULDER_DRIVE = 0.425; // 0.425
private static final double SCORE_ONE_SHOULDER = Mutation.SCORE_ONE_SHOULDER;
private static final double SCORE_ONE_WRIST = Mutation.SCORE_ONE_WRIST;
private static final double SCORE_ONE_ELBOW = Mutation.SCORE_ONE_ELBOW;
private static final double SCORE_ONE_SHOULDER = Evolution.SCORE_ONE_SHOULDER;
private static final double SCORE_ONE_WRIST = Evolution.SCORE_ONE_WRIST;
private static final double SCORE_ONE_ELBOW = Evolution.SCORE_ONE_ELBOW;

public static final double SHOULDER_TOP_TWO = Evolution.SHOULDER_TOP_TWO;
public static final double WRIST_TOP_TWO = Evolution.WRIST_TOP_TWO;
public static final double ELBOW_TOP_TWO = Evolution.ELBOW_TOP_TWO;

private static final double PIXEL_STACK_FINGER_GRAB = 0.6;


//Define and declare Robot Starting Locations
Expand Down Expand Up @@ -173,13 +179,13 @@ public void runOpMode() throws InterruptedException {


//init pos
setLiftPosition(Mutation.LIFT_DRIVE);
shoulder.setPosition(Mutation.SHOULDER_DRIVE);
wrist.setPosition(Mutation.WRIST_INTAKE);
elbow.setPosition(Mutation.ELBOW_DRIVE);
setLiftPosition(Evolution.LIFT_DRIVE);
shoulder.setPosition(Evolution.SHOULDER_DRIVE);
wrist.setPosition(Evolution.WRIST_INTAKE);
elbow.setPosition(Evolution.ELBOW_DRIVE);
sleep(2500);
leftFinger.setPosition(Mutation.LEFT_FINGER_GRIP);
rightFinger.setPosition(Mutation.RIGHT_FINGER_GRIP);
leftFinger.setPosition(Evolution.LEFT_FINGER_GRIP);
rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP);

// Vision OpenCV / Color Detection
WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
Expand Down Expand Up @@ -256,6 +262,7 @@ public void runAutonoumousMode() {
Pose2d midwayPose1 = new Pose2d(0,0,0);
Pose2d midwayPose1a = new Pose2d(0,0,0);
Pose2d intakeStack = new Pose2d(0,0,0);
Pose2d intakeStack2 = new Pose2d(0,0,0);
Pose2d midwayPose2 = new Pose2d(0,0,0);
Pose2d dropYellowPixelPose = new Pose2d(0, 0, 0);
Pose2d parkPose = new Pose2d(0,0, 0);
Expand Down Expand Up @@ -287,7 +294,8 @@ public void runAutonoumousMode() {
break;
}
midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45));
waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board
waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board
//used to be 2 ^ this goes for all of them
parkPose = new Pose2d(8, 30, Math.toRadians(-90));
break;

Expand All @@ -311,7 +319,7 @@ public void runAutonoumousMode() {
break;
}
midwayPose1 = new Pose2d(14, -13, Math.toRadians(45));
waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board
waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board
parkPose = new Pose2d(8, -30, Math.toRadians(90));
break;

Expand All @@ -336,9 +344,10 @@ public void runAutonoumousMode() {
}
midwayPose1 = new Pose2d(8, -8, Math.toRadians(0));
midwayPose1a = new Pose2d(18, -21, Math.toRadians(-90));
intakeStack = new Pose2d(52, -19,Math.toRadians(-90));
intakeStack = new Pose2d(53.25, -15,Math.toRadians(-90));
intakeStack2 = new Pose2d(53.25, -19.25,Math.toRadians(-90));
midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90));
waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board
waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board
parkPose = new Pose2d(50, 84, Math.toRadians(-90));
break;

Expand All @@ -365,7 +374,7 @@ public void runAutonoumousMode() {
midwayPose1a = new Pose2d(18, 21, Math.toRadians(90));
intakeStack = new Pose2d(52, 19,Math.toRadians(90));
midwayPose2 = new Pose2d(52, -62, Math.toRadians(90));
waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board
waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board
parkPose = new Pose2d(50, -84, Math.toRadians(90));
break;
}
Expand All @@ -380,18 +389,18 @@ public void runAutonoumousMode() {
.build());

//TODO : Code to drop Purple Pixel on Spike Mark
safeWaitSeconds(1);
shoulder.setPosition(Mutation.SHOULDER_DRIVE);
wrist.setPosition(Mutation.WRIST_INTAKE);
safeWaitSeconds(0);
shoulder.setPosition(Evolution.SHOULDER_DRIVE);
wrist.setPosition(Evolution.WRIST_INTAKE);
for(int c = 0; c<40; c++) {
moveServoGradually(elbow, ELBOW_INTAKE);
sleep(10);
}
sleep(200);
rightFinger.setPosition(Mutation.RIGHT_FINGER_DROP);
rightFinger.setPosition(Evolution.RIGHT_FINGER_DROP);
sleep(200);
elbow.setPosition(ELBOW_DRIVE);
wrist.setPosition(Mutation.WRIST_TUCK);
wrist.setPosition(Evolution.WRIST_TUCK);

//Move robot to midwayPose1
Actions.runBlocking(
Expand All @@ -410,6 +419,31 @@ public void runAutonoumousMode() {

//TODO : Code to intake pixel from stack
safeWaitSeconds(1);
shoulder.setPosition(SHOULDER_TOP_TWO);
wrist.setPosition(WRIST_TOP_TWO);
elbow.setPosition(ELBOW_TOP_TWO);
for(int e = 0; e<40; e++) {
moveServoGradually(elbow, ELBOW_TOP_TWO);
sleep(10);
}
for(int w = 0; w<40; w++) {
moveServoGradually(wrist, WRIST_TOP_TWO);
sleep(10);
}
for(int s = 0; s<40; s++) {
moveServoGradually(shoulder, SHOULDER_TOP_TWO);
sleep(10);
}
Actions.runBlocking(
drive.actionBuilder(drive.pose)
.strafeToLinearHeading(intakeStack2.position, intakeStack2.heading)
.build());
sleep(300);
rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB);
sleep(500);
elbow.setPosition(ELBOW_DRIVE);
wrist.setPosition(Evolution.WRIST_TUCK);
sleep(500);

//Move robot to midwayPose2 and to dropYellowPixelPose
Actions.runBlocking(
Expand All @@ -429,7 +463,7 @@ public void runAutonoumousMode() {


//TODO : Code to drop Pixel on Backdrop
safeWaitSeconds(1);
safeWaitSeconds(0);
for(int w = 0; w<40; w++) {
moveServoGradually(wrist, SCORE_ONE_WRIST);
sleep(10);
Expand All @@ -442,11 +476,11 @@ public void runAutonoumousMode() {
moveServoGradually(shoulder, SCORE_ONE_SHOULDER);
sleep(7);
}
sleep(200);
leftFinger.setPosition(Mutation.LEFT_FINGER_DROP);
sleep(100);
sleep(50);
leftFinger.setPosition(Evolution.LEFT_FINGER_DROP);
sleep(50);
for(int s = 0; s<200; s++) {
moveServoGradually(shoulder, Mutation.SHOULDER_DRIVE);
moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE);
sleep(7);
}
for(int w = 0; w<40; w++) {
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@
/**
* ENIGMA Autonomous Example for only vision detection using openCv and park
*/
@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Mutation")
@Autonomous(name = "ENIGMA Autonomous Mode - Backup", group = "00-Autonomous", preselectTeleOp = "Mutation")
public class EnigmaAuto2QualBackup extends LinearOpMode {

public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name
Expand Down
3 changes: 2 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,13 @@

import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

//@TeleOp
@TeleOp
public class Evolution extends LinearOpMode {

// Declare vars
Expand Down
2 changes: 2 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,12 @@
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;

@TeleOp
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
Expand Down

0 comments on commit 631fe1d

Please sign in to comment.