Skip to content

Code #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
RayHCai wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Code #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 23 additions & 11 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

// Commands
Expand All @@ -18,7 +19,7 @@
import edu.wpi.first.wpilibj.Timer;

public class RobotContainer {
private static final double ARM_SPEED = 0.25;
private static final double ARM_SPEED = 0.5;

// Controllers
private final CommandXboxController driver = new CommandXboxController(0);
Expand All @@ -40,7 +41,7 @@ public class RobotContainer {

public RobotContainer() {
// Move the arm up and down
Arm armCommand = new Arm(
MoveArm armCommand = new MoveArm(
armSubsystem,
() -> (
operator.leftBumper().getAsBoolean()
Expand All @@ -53,23 +54,34 @@ public RobotContainer() {

armSubsystem.setDefaultCommand(armCommand);

// Move the arm to the amp
Arm amp = new Arm(armSubsystem, () -> 5, () -> 0.11899);
operator.b().whileTrue(amp);
// Amp
operator.y().whileTrue(
new SequentialCommandGroup(
new MoveArm(armSubsystem, () -> 5, () -> 0.10),
new Shoot(shootSubsystem, () -> 1),
new Intake(intakeSubsystem, () -> 1)
)
);

// Move the arm to the speaker
Arm speaker = new Arm(armSubsystem, () -> 5, () -> 0.33);
operator.x().whileTrue(speaker);
// Speaker
operator.x().whileTrue(
new MoveArm(armSubsystem, () -> 5, () -> 0.33)
// new SequentialCommandGroup(
// new Shoot(shootSubsystem, () -> 1),
// new Intake(intakeSubsystem, () -> 1)
// )
);

// Intake
Intake intakeCommand = new Intake(intakeSubsystem, () -> operator.getRawAxis(leftJoystick));
Intake intakeCommand = new Intake(intakeSubsystem, () -> operator.getRawAxis(leftJoystick)*.8);

intakeSubsystem.setDefaultCommand(intakeCommand);

// Shoot
Shoot shootCommand = new Shoot(
shootSubsystem,
() -> operator.getRawAxis(rightJoystick));
shootSubsystem,
() -> operator.getRawAxis(rightJoystick)
);

shootSubsystem.setDefaultCommand(shootCommand);

Expand Down
75 changes: 53 additions & 22 deletions src/main/java/frc/robot/robot/commands/Auton.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,19 +58,7 @@ private double getRotation(DoubleSupplier rotationSup) {
return MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband);
}

private void shootSpeaker(Timer timer) {
if (timer.get() < 2) {
armSubsystem.moveArm(1, 0.32);
}
else if(timer.get() < 3) {
shootSubsystem.shoot(1);
}
else if(timer.get() < 4) {
intakeSubsystem.intake(1);
}
}

// Left side of speaker
// Short side of speaker
private void auton1() {
if (timer.get() < 2) {
armSubsystem.moveArm(1, 0.32);
Expand All @@ -81,22 +69,44 @@ else if(timer.get() < 3) {
else if(timer.get() < 4) {
intakeSubsystem.intake(1);
}
else if(timer.get() < 5) {
intakeSubsystem.stopIntake();
else if(timer.get() < 5.2) {
shootSubsystem.stopShoot();
armSubsystem.stopArm();

swerveSubsystem.drive(
getTranslation(
() -> -0.5,
() -> 0.1),
getRotation(() -> 0),
() -> -.6, //-0.6 backwards
() -> .6), // right
getRotation(() -> -9), //0
true,
true
);
}
else if(timer.get() < 7) {
swerveSubsystem.drive(
getTranslation(
() -> 0.25,
() -> 0.2),
getRotation(() -> 2
),
true,
true
);
intakeSubsystem.stopIntake();
}
else if(timer.get() < 7.5) {
intakeSubsystem.intake(-0.1);
}
else if(timer.get() < 8) {
swerveSubsystem.stopDrive();
shootSubsystem.shoot(1);
}
else if(timer.get() < 8.5) {
intakeSubsystem.intake(1);
}
else if(timer.get() < 10) {
intakeSubsystem.stopIntake();
shootSubsystem.stopShoot();
}
}

Expand Down Expand Up @@ -151,7 +161,7 @@ else if(timer.get() < 8) {
}
}

// Right side of speaker
// Long side of speaker
private void auton3() {
if (timer.get() < 2) {
armSubsystem.moveArm(1, 0.32);
Expand All @@ -163,21 +173,42 @@ else if(timer.get() < 4) {
intakeSubsystem.intake(1);
}
else if(timer.get() < 5) {
intakeSubsystem.stopIntake();
shootSubsystem.stopShoot();
armSubsystem.stopArm();

swerveSubsystem.drive(
getTranslation(
() -> -0.5,
() -> -0.5),
() -> -0.6,
() -> -0.6),
getRotation(() -> 0),
true,
true
);
}
else if(timer.get() < 7) {
swerveSubsystem.drive(
getTranslation(
() -> 0.25,
() -> 0.2),
getRotation(() -> 0),
true,
true
);
intakeSubsystem.stopIntake();
}
else if(timer.get() < 7.5) {
intakeSubsystem.intake(-0.1);
}
else if(timer.get() < 8) {
swerveSubsystem.stopDrive();
shootSubsystem.shoot(1);
}
else if(timer.get() < 8.5) {
intakeSubsystem.intake(1);
}
else if(timer.get() < 10) {
intakeSubsystem.stopIntake();
shootSubsystem.stopShoot();
}
}

Expand Down
6 changes: 3 additions & 3 deletions ...in/java/frc/robot/robot/commands/Arm.java → ...ava/frc/robot/robot/commands/MoveArm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,20 +7,20 @@

import frc.robot.robot.subsystems.arm.ArmSubsystem;

public class Arm extends Command {
public class MoveArm extends Command {
private DoubleSupplier speedSup;
private DoubleSupplier angleSup;

private ArmSubsystem armSubsystem;

public Arm(ArmSubsystem subsystem, DoubleSupplier speedSup) {
public MoveArm(ArmSubsystem subsystem, DoubleSupplier speedSup) {
this.armSubsystem = subsystem;
addRequirements(armSubsystem);

this.speedSup = speedSup;
}

public Arm(ArmSubsystem subsystem, DoubleSupplier speedSup, DoubleSupplier angleSup) {
public MoveArm(ArmSubsystem subsystem, DoubleSupplier speedSup, DoubleSupplier angleSup) {
this.armSubsystem = subsystem;
addRequirements(armSubsystem);

Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/robot/subsystems/arm/ArmSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,11 +23,15 @@ public ArmSubsystem() {
}

public void moveArm(double speed) {
if(getArmEncoder() < 0.09 && speed < 0) return;

leftArmMotor.set(ControlMode.PercentOutput, speed);
rightArmMotor.set(ControlMode.PercentOutput, -speed);
}

public void moveArm(double speed, double targetAngle) {
if(targetAngle < 0.09 && speed < 0) return;

double encoderVal = armEncoder.getOutput();

leftArmMotor.set(ControlMode.PercentOutput, (targetAngle - encoderVal) * speed);
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/robot/subsystems/swerve/rev/RevSwerveConfig.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,9 +99,9 @@ public class RevSwerveConfig {

/* Swerve Profiling Values */
/** Meters per Second */
public static final double maxSpeed = 2;
public static final double maxSpeed = 20;
/** Radians per Second */
public static final double maxAngularVelocity = 9;
public static final double maxAngularVelocity = 20;

public RevSwerveConfig() {
canCoderConfig = new CANcoderConfiguration();
Expand Down