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

fixed teleop things, started new auto and varun is bad

This commit is contained in:
Robert Teal 2023-12-20 18:10:59 -06:00
parent 4625c3f784
commit 5eb278042b
9 changed files with 201 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,8 +24,8 @@ public class Arm {
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 doorOpenPos = 0.3;
public static double doorClosePos = 0.61;
public static double armStart = 0.24;
public static double armScore = 0.95;

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

@ -1,25 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.util.Constants;
public class DroneLauncher {
private Servo servo;
public static double initPos = 0;
public static double launchPos = 0.5;
public DroneLauncher(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, "Drone Launcher");
this.servo.setPosition(initPos);
}
public void launch() {
this.servo.setPosition(launchPos);
}
public void reset() {
this.servo.setPosition(initPos);
}
}

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

@ -18,7 +18,7 @@ public class Robot {
public Intake intake;
public Slides slides;
public Arm arm;
public DroneLauncher droneLauncher;
public endGame_Mechs endGameMechs;
public double macroStartTime = 0;
public int macroState = 0;
@ -28,7 +28,7 @@ public class Robot {
private boolean camEnabled = true;
public static double slideWait = 1.5;
public static double scoreWait = 1.25;
public static double scoreWait = 0.65;
public static double armWait = 2.0;
//Name the objects
@ -39,7 +39,7 @@ public class Robot {
intake = new Intake(hardwareMap, UP);
slides = new Slides(hardwareMap);
arm = new Arm(hardwareMap);
droneLauncher = new DroneLauncher(hardwareMap);
endGameMechs = new endGame_Mechs(hardwareMap);
camEnabled = true;
}
@ -71,6 +71,7 @@ public class Robot {
case(0):
macroStartTime = runTime;
arm.setDoor(OPEN);
slides.setTarget(slides.getTarget()+70);
macroState++;
break;
case(1):
@ -104,6 +105,6 @@ public class Robot {
}
public String getTelemetry() {
return arm.getTelemetry();
return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s", arm.getTelemetry(), slides.getTelemetry());
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -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 = 555;
public static int targetMax = 830;
public static int down = 0;
public static int tier1 = 350;
public static int tier2 = 450;
public static int tier3 = 550;
public static int tier2 = 500;
public static int tier3 = 760;
private int target = 0;
@ -71,6 +71,8 @@ public class Slides {
return target;
}
public boolean atTarget() {
return controller.atSetPoint();
}
@ -114,6 +116,6 @@ public class Slides {
}
public String getTelemetry() {
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
}
}

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java Normal file
View File

@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@Config
public class endGame_Mechs {
private Servo servo;
private Servo hang1;
private Servo hang2;
public static double initPos = 0;
public static double launchPos = 0.5;
public static double hold = 0;
public static double release = 0.5;
public static double hold2 = 0;
public static double release2 = 0.5;
public endGame_Mechs(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, "Drone Launcher");
this.servo.setPosition(initPos);
this.hang1 = hardwareMap.get(Servo.class, "Hanger 1");
this.hang1.setPosition(hold);
this.hang2 = hardwareMap.get(Servo.class, "Hanger 2");
this.hang2.setPosition(hold);
}
public void launch() {
this.servo.setPosition(launchPos);
}
public void reset() {
this.servo.setPosition(initPos);
}
public void release() {
this.servo.setPosition(release);
this.servo.setPosition(release2);
}
public void hold() {
this.servo.setPosition(hold);
this.servo.setPosition(hold2);
}
}

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

@ -84,10 +84,6 @@ public abstract class AbstractAuto extends LinearOpMode {
step.start();
}
// // update turret and slides position
// PoseStorage.slidesPosition = robot.actuators.getSlides();
// PoseStorage.turretPosition = robot.actuators.getTurret();
// while the step is running display telemetry
step.whileRunning();
robot.update(currentRuntime);
@ -262,6 +258,7 @@ public abstract class AbstractAuto extends LinearOpMode {
@Override
public void end() {
robot.intake.setDcMotor(0);
robot.intake.setpos(Intake.Position.UP);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
}

55
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java
View File

@ -15,6 +15,8 @@ public class RedRightAuto extends AbstractAuto {
public Trajectory scoreYellow;
public Trajectory load1;
public Trajectory score1;
public Trajectory load2;
public Trajectory score2;
public Trajectory parkRobot;
@Override
@ -31,6 +33,12 @@ public class RedRightAuto extends AbstractAuto {
Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180));
Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180));
Pose2d lineup2 = new Pose2d(12, -14, Math.toRadians(180));
Pose2d pickup2 = new Pose2d(-58, -14, Math.toRadians(180));
Pose2d driveup2 = new Pose2d(12-6, -14, Math.toRadians(180));
Pose2d deposit2 = new Pose2d(48-6, -36-4, Math.toRadians(180));
Pose2d park = new Pose2d(48, -12, Math.toRadians(180));
scorePurple = robot.drive.trajectoryBuilder(start)
@ -41,29 +49,53 @@ public class RedRightAuto extends AbstractAuto {
.lineToLinearHeading(depositPreload)
.build();
int driveSpeed = 45;
load1 = robot.drive.trajectoryBuilder(scoreYellow.end())
.splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, 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.getVelocityConstraint(driveSpeed, 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.getVelocityConstraint(driveSpeed, 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.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
parkRobot = robot.drive.trajectoryBuilder(score1.end())
load2 = robot.drive.trajectoryBuilder(score1.end())
.splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.lineToLinearHeading(pickup1,
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
score2 = robot.drive.trajectoryBuilder(load2.end(), true)
.lineToLinearHeading(driveup1,
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
)
.build();
parkRobot = robot.drive.trajectoryBuilder(score2.end())
.lineToLinearHeading(park)
.build();
@ -72,14 +104,19 @@ public class RedRightAuto extends AbstractAuto {
@Override
public void initializeSteps(int location) {
// score preloads
followTrajectory(scorePurple);
runIntake(-0.4, 0.5);
// followAndExtend(scoreYellow, Slides.Position.TIER1);
// followAndRetract(load1, Slides.Position.DOWN);
followTrajectory(scoreYellow);
followTrajectory(load1);
followAndExtend(scoreYellow, Slides.Position.TIER1);
// cycle
followAndRetract(load1, Slides.Position.DOWN);
intakeStack(Intake.Position.STACK5, Intake.Position.STACK4);
followAndExtend(score1, Slides.Position.TIER1);
followAndRetract(load2, Slides.Position.DOWN);
intakeStack(Intake.Position.STACK3, Intake.Position.STACK2);
followAndExtend(score2, Slides.Position.TIER1);
followAndRetract(parkRobot, Slides.Position.DOWN);
}

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

@ -0,0 +1,64 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@Autonomous (name= "varun_is_Dumb")
public class vannobAuto_RightRed extends LinearOpMode {
public Robot robot;
int teamElementLocation = 2;
int macrostate = 0;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
// make trajectories
while (!(isStarted() || isStopRequested())) {
double currentRuntime = getRuntime();
robot.update(currentRuntime);
// int newLocation = robot.camera.getMarkerId();
// if (newLocation != -1) {
// teamElementLocation = newLocation;
// }
telemetry.addLine("Initialized");
telemetry.addLine("Randomization of T.O.M: "+teamElementLocation);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
while(macrostate < 6 || !isStopRequested()){
//update everything
robot.update(getRuntime());
switch (macrostate){
case (0):
//follow trajectory
macrostate++;
break;
case (1):
// run intake
macrostate++;
break;
case (2):
macrostate++;
break;
case (3):
macrostate++;
break;
case (4):
macrostate++;
break;
case (5):
macrostate++;
break;
}
}
}
}

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -9,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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.Slides;
@ -19,7 +18,9 @@ public class MainTeleOp extends OpMode {
public static double normal = 0.5;
public static double turbo = 1;
public static double slow_mode = 0.15;
public static double intakeMax = 0.65;
public static double intakeMax2 = -0.65;
private Robot robot;
private Controller controller1;
@ -32,7 +33,7 @@ public class MainTeleOp extends OpMode {
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
this.robot.endGameMechs.hold();
while (robot.camera.getFrameCount() < 1) {
telemetry.addLine("Initializing...");
telemetry.update();
@ -40,6 +41,8 @@ public class MainTeleOp extends OpMode {
telemetry.addLine("Initialized");
telemetry.update();
}
@Override
@ -53,6 +56,11 @@ public class MainTeleOp extends OpMode {
x *= turbo;
y *= turbo;
z *= turbo;
}
else if (controller1.getLeftTrigger().getValue() > 0.1) {
x *= slow_mode;
y *= slow_mode;
z *= slow_mode;
} else {
x *= normal;
y *= normal;
@ -61,7 +69,16 @@ public class MainTeleOp extends OpMode {
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Driver 2
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
if (controller2.getRightTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
}
else if(controller2.getLeftTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
}
else {
robot.intake.setDcMotor(0);
}
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
@ -71,10 +88,15 @@ public class MainTeleOp extends OpMode {
// Drone launcher
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.droneLauncher.launch();
this.robot.endGameMechs.launch();
} else {
this.robot.droneLauncher.reset();
this.robot.endGameMechs.reset();
}
//Hang Servos
if (controller1.getX().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.release();
}
// macros
switch (robot.runningMacro) {