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

I added the new door to the macros and a potential blue right auto drive path.

This commit is contained in:
ImposterVarunoverlord 2023-11-28 11:20:55 -06:00
parent 5329732517
commit a13a1d584b
7 changed files with 120 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,7 +65,7 @@ public class Arm {
}
}
public void openDoor(DoorPos.DoorPosition pos) {
public void setDoor(DoorPos.DoorPosition pos) {
if (pos == DoorPos.DoorPosition.OPEN) {
dServo.setPosition(doorOpenpos);
} else if (pos == DoorPos.DoorPosition.CLOSE) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java
View File

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

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

@ -76,6 +76,10 @@ public class Intake {
dcMotor.setPower(pwr);
}
public double getPower() {
return dcMotor.getPower();
}
public String getTelemetry() {
return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower();
}

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

@ -1,6 +1,8 @@
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 com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -41,6 +43,7 @@ public class Robot {
case(0):
macroStartTime = runTime;
slides.setTarget(slidePos);
arm.setDoor(CLOSE);
macroState ++;
break;
case(1):
@ -63,16 +66,27 @@ public class Robot {
switch(macroState) {
case(0):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
arm.setDoor(OPEN);
macroState++;
break;
//Ind_sleeper
case(1):
if (runTime > macroStartTime + 1) {
if (runTime > macroStartTime + 2) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
macroState++;
break;
case(3):
if (runTime > macroStartTime + 1.1) {
macroState ++;
}
break;
case(4):
macroStartTime = runTime;
slides.setTarget(pos);
macroState = 0;

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

@ -0,0 +1,79 @@
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 org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "BlueRightAuto(Drive only)")
public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// add more trajectories here
Pose2d start = new Pose2d(-24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(-24, 40.5, Math.toRadians(-180));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d stack1 = new Pose2d(-68, 40.5, Math.toRadians(-180));
Pose2d dropMiddle2 = new Pose2d(-24, 40.5, Math.toRadians(-180));
Pose2d bmid = new Pose2d(-24, 5, Math.toRadians(-180));
Pose2d bmid2 = new Pose2d(24, 5, Math.toRadians(-180));
Pose2d alimb = new Pose2d(60, 40.5, Math.toRadians(-180));
Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
// add this per trajectories
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
Trajectory stack= drive.trajectoryBuilder(dropMiddle)
.lineToLinearHeading(stack1)
.build();
Trajectory back_to_mid= drive.trajectoryBuilder(stack1)
.lineToLinearHeading(dropMiddle2)
.build();
Trajectory front_gate= drive.trajectoryBuilder(dropMiddle2)
.lineToLinearHeading(bmid)
.build();
Trajectory front_gate_pt_2= drive.trajectoryBuilder(bmid)
.lineToLinearHeading(bmid2)
.build();
Trajectory about_to_score= drive.trajectoryBuilder(bmid2)
.lineToLinearHeading(alimb)
.build();
// the scorePurple. should be whatever the start pose2d thing was
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
waitForStart();
if(isStopRequested()) return;
// add this per trajectories
drive.followTrajectory(scorePurple);
drive.followTrajectory(stack);
drive.followTrajectory(back_to_mid);
drive.followTrajectory(front_gate);
drive.followTrajectory(front_gate_pt_2);
drive.followTrajectory(about_to_score);
drive.followTrajectory(scoreYellow);
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java
View File

@ -8,11 +8,11 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "Simple Blue Left Auto")
public class SimpleBlueLeftAuto extends LinearOpMode {
public class SimpleBlueLeftAuto_ignore extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// add more trajectories here
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
@ -22,11 +22,11 @@ public class SimpleBlueLeftAuto extends LinearOpMode {
Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
// add this per trajectories
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
// the scorePurple. should be whatever the start pose@d thing was
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
@ -34,7 +34,7 @@ public class SimpleBlueLeftAuto extends LinearOpMode {
waitForStart();
if(isStopRequested()) return;
// add this per trajectories
drive.followTrajectory(scorePurple);
// drive.wait(1);
drive.followTrajectory(scoreYellow);

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java
View File

@ -1,10 +1,12 @@
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 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.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Intake;
@ -63,7 +65,7 @@ public class NewTeleop extends OpMode {
// macros
switch (robot.runningMacro) {
case(0): // manual mode
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
@ -74,17 +76,22 @@ public class NewTeleop extends OpMode {
} else if (controller2.getA().isJustPressed()) {
robot.runningMacro = 4;
}
if (robot.intake.getPower() >= 0.01) {
robot.arm.setDoor(OPEN);
} else {
robot.arm.setDoor(CLOSE);
}
break;
case(1):
case (1):
robot.extendMacro(Slides.Position.TIER1, getRuntime());
break;
case(2):
case (2):
robot.extendMacro(Slides.Position.TIER2, getRuntime());
break;
case(3):
case (3):
robot.extendMacro(Slides.Position.TIER3, getRuntime());
break;
case(4):
case (4):
robot.resetMacro(Slides.Position.DOWN, getRuntime());
break;
}