Skip to content

Commit

Permalink
Merge pull request #8 from fusion479/qualOne
Browse files Browse the repository at this point in the history 
messy code after qual 1
  • Loading branch information
@Bi1ku
Bi1ku authored Dec 9, 2024
2 parents 5d6d7d4 + 76e7d65 commit dadbe17
Show file tree
Hide file tree
Showing 7 changed files with 61 additions and 31 deletions.
46 changes: 27 additions & 19 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandRobot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.command.Command;
import com.arcrobotics.ftclib.command.ParallelCommandGroup;
import com.arcrobotics.ftclib.command.SequentialCommandGroup;
import com.arcrobotics.ftclib.command.WaitCommand;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
Expand All @@ -26,7 +25,6 @@
import org.firstinspires.ftc.teamcode.commands.lift.LiftIncrement;
import org.firstinspires.ftc.teamcode.commands.lift.LiftLowBasket;
import org.firstinspires.ftc.teamcode.commands.lift.LiftLowRung;
import org.firstinspires.ftc.teamcode.commands.lift.LiftSlam;
import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Claw;
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
Expand All @@ -41,7 +39,7 @@ public class CommandRobot {
private final Extendo extendo;
private final Claw claw;
private final OpModeCore opMode;
public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, open, close;
public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, open, close, slamdown, specimen;
private Drivetrain drivetrain;
private GamepadEx gamepad1;
private GamepadEx gamepad2;
Expand Down Expand Up @@ -87,36 +85,37 @@ public MecanumDrive getDrive() {
}

public void startThreads() {
this.drivetrain.startThread(this.gamepad2, this.opMode);
this.drivetrain.startThread(this.gamepad1, this.opMode);
this.lift.startThread(this.opMode);
// TODO: Add in more threads if needed
}

public void configureCommands() {
this.ready = new SequentialCommandGroup(
new LiftAccepting(this.telemetry, this.lift),
new ClawClose(this.telemetry, this.claw),
new ClawPivotScore(this.telemetry, this.claw),
new WaitCommand(CommandRobot.CLAW_DEPLOY_DELAY),
new ExtendoReady(this.telemetry, this.extendo)
new WaitCommand(1000),
new ExtendoReady(this.telemetry, this.extendo),
new LiftAccepting(this.telemetry, this.lift)

);

this.accepting = new SequentialCommandGroup(
new LiftAccepting(this.telemetry, this.lift),
new WaitCommand(CommandRobot.CLAW_DEPLOY_DELAY - 100),
new ExtendoAccepting(this.telemetry, this.extendo),
new WaitCommand(CommandRobot.CLAW_DEPLOY_DELAY),
new ClawOpen(this.telemetry, this.claw),
new ClawPivotAccepting(this.telemetry, this.claw)
);

this.highBasket = new ParallelCommandGroup(
this.highBasket = new SequentialCommandGroup(
new LiftHighBasket(this.telemetry, this.lift),
new WaitCommand(CommandRobot.LIFT_DELAY + 300),
new ExtendoScore(this.telemetry, this.extendo),
new ClawPivotScore(this.telemetry, this.claw)
);

this.lowBasket = new ParallelCommandGroup(
this.lowBasket = new SequentialCommandGroup(
new LiftLowBasket(this.telemetry, this.lift),
new WaitCommand(CommandRobot.LIFT_DELAY + 100),
new ExtendoScore(this.telemetry, this.extendo),
Expand All @@ -137,6 +136,15 @@ public void configureCommands() {
new ClawPivotScore(this.telemetry, this.claw)
);

this.specimen = new SequentialCommandGroup(
new LiftLowRung(this.telemetry, this.lift),
new WaitCommand(CommandRobot.LIFT_DELAY),
new ExtendoReady(this.telemetry, this.extendo),
new ClawPivotScore(this.telemetry, this.claw),
new ClawOpen(this.telemetry, this.claw)
);


this.liftIncrement = new LiftIncrement(this.telemetry, this.lift);

this.liftDecrement = new LiftDecrement(this.telemetry, this.lift);
Expand All @@ -148,25 +156,25 @@ public void configureCommands() {

// TODO: Configure controls for gamepad (talk with drive team)
public void configureControls() {
this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.accepting);
this.gamepad2.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.highBasket);
this.gamepad2.getGamepadButton(GamepadKeys.Button.A)
// this.gamepad2.getGamepadButton(GamepadKeys.Button.Y)
// .whenPressed(this.highBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.ready);
this.gamepad2.getGamepadButton(GamepadKeys.Button.Y)
this.gamepad2.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.lowBasket);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenPressed(this.liftIncrement);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
.whenPressed(this.liftDecrement);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)
this.gamepad2.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.lowRung);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.highRung);
this.gamepad2.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(this.open);
this.gamepad2.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(this.close);
}
}
23 changes: 20 additions & 3 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/Park.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,35 +2,52 @@

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.command.CommandScheduler;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Claw;
import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;

@Config
@Autonomous(name = "Park", preselectTeleOp = "Main")
public class Park extends OpModeCore {
private MecanumDrive drive;
private ElapsedTime timer;
private Claw claw;

public static int DURATION = 5000;
public static int DURATION = 3000;

@Override
public void initialize() {
this.drive = new MecanumDrive(super.hardwareMap, new Pose2d(0, 0, 0));
this.timer = new ElapsedTime();

this.claw = new Claw(super.hardwareMap, super.multipleTelemetry);
}

@Override
public void runOpMode() {
CommandScheduler.getInstance().enable();
this.initialize();

super.waitForStart();

this.timer.reset();
while (super.opModeIsActive()) {
CommandScheduler.getInstance().run();
super.telemetry.addData("TIMER", this.timer.milliseconds());
super.telemetry.addData("CONDITION", this.timer.milliseconds() <= Park.DURATION);

if (this.timer.milliseconds() <= Park.DURATION) {
this.drive.setPowers(1.0, 1.0, 1.0, 1.0);
this.drive.setPowers(-0.25, -0.25, -0.25, -0.25);
} else {
this.drive.setPowers(0, 0, 0, 0);
}
super.telemetry.update();
}

super.end();

}
}
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,10 +129,10 @@ public void setDrivePowers(PoseVelocity2d powers) {
maxPowerMag = Math.max(maxPowerMag, power.value());
}

leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag * 0.85);
leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag * 0.85);
}

public PoseVelocity2d updatePoseEstimate() {
Expand Down
5 changes: 4 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Claw.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ public class Claw extends SubsystemBase {
public static double ACCEPTING = 0.55;

public static double OPEN = 1;
public static double CLOSE = 0.52;
public static double CLOSE = 0.49;

private final MultipleTelemetry telemetry;

Expand All @@ -24,6 +24,9 @@ public Claw(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
this.leftPivot = hwMap.get(Servo.class, "leftPivot");
this.rightPivot = hwMap.get(Servo.class, "rightPivot");
this.claw = hwMap.get(Servo.class, "claw");

this.setPivotPosition(Claw.SCORE);
this.setClawPosition(Claw.CLOSE);
}

public void setPivotPosition(double position) {
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,13 @@
@Config
public class Drivetrain extends SubsystemBase {
public static double MAX_ACCEL = 0.3;
public static double MAX_ANGULAR_ACCEL = 0.3;
public static double MAX_ANGULAR_ACCEL = 0.1;

public static double MAX_DEACCEL = 0.5;
public static double MAX_ANGULAR_DEACCEL = 0.5;

public static double MAX_VEL = 0.75;
public static double MAX_ANGULAR_VEL = 0.75;
public static double MAX_ANGULAR_VEL = 0.6;

private final MecanumDrive drive;
private final MultipleTelemetry telemetry;
Expand Down
2 changes: 2 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Extendo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@ public Extendo(final HardwareMap hwMap, final MultipleTelemetry telemetry) {
this.telemetry = telemetry;

this.extendo = hwMap.get(Servo.class, "extendo");

this.extendo.setPosition(Extendo.READY);
}

public double getPosition() {
Expand Down
8 changes: 4 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,16 +20,16 @@ public class Lift extends SubsystemBase {

public static double ACCEPTING = 50;
public static double LOW_BASKET = 1500;
public static double HIGH_BASKET = 2600; // higher
public static double HIGH_BASKET = 2800;

public static double LOW_RUNG = 1000;
public static double HIGH_RUNG = 2050;
public static double HIGH_RUNG = 2100;
public static double INCREMENT = 250;

public static double kP = 0.0018;
public static double kP = 0.0015;
public static double kI = 0.0006;
public static double kD = 0.000015;
public static double kG = 0.06;
public static double kG = 0.063;

private final DcMotorEx rightMotor;
private final DcMotorEx leftMotor;
Expand Down

0 comments on commit dadbe17

Please sign in to comment.