Skip to content

Commit 56686e8

Browse files
rl507rl507
committed
Merge branch 'master' into rliu/subsystemConfig
2 parents 65ba75a + 9577066 commit 56686e8

File tree

48 files changed

+849
-295
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

48 files changed

+849
-295
lines changed

‎docs/Subsystem-Interface-Conversions.xlsx‎

11 Bytes
Binary file not shown.

‎src/main/java/frc/team2767/deepspace/Robot.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
import edu.wpi.first.wpilibj.command.CommandGroup;
77
import edu.wpi.first.wpilibj.command.Scheduler;
88
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
9-
import frc.team2767.deepspace.command.sequences.SandstormHatchPickupCommandGroup;
9+
import frc.team2767.deepspace.command.sequences.pickup.SandstormHatchPickupCommandGroup;
1010
import frc.team2767.deepspace.control.Controls;
1111
import frc.team2767.deepspace.subsystem.*;
1212
import frc.team2767.deepspace.subsystem.safety.SafetySubsystem;

‎src/main/java/frc/team2767/deepspace/command/TeleOpDriveCommand.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public final class TeleOpDriveCommand extends Command {
1515
private static final DriveSubsystem DRIVE = Robot.DRIVE;
1616
private static DriverControls controls;
1717

18-
private static final double DEADBAND = 0.09;
18+
private static final double DEADBAND = 0.05;
1919

2020
public TeleOpDriveCommand() {
2121
requires(DRIVE);

‎src/main/java/frc/team2767/deepspace/command/approach/ApproachDirectionCommand.java‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
package frc.team2767.deepspace.command.approach;
2+
3+
import edu.wpi.first.wpilibj.command.InstantCommand;
4+
import frc.team2767.deepspace.Robot;
5+
import frc.team2767.deepspace.subsystem.DriveSubsystem;
6+
import frc.team2767.deepspace.subsystem.FieldDirection;
7+
import frc.team2767.deepspace.subsystem.VisionSubsystem;
8+
9+
public class ApproachDirectionCommand extends InstantCommand {
10+
11+
private static final VisionSubsystem VISION = Robot.VISION;
12+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
13+
14+
public ApproachDirectionCommand() {}
15+
16+
@SuppressWarnings("Duplicates")
17+
@Override
18+
protected void initialize() {
19+
double bearing = Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360);
20+
if (bearing <= 0) {
21+
VISION.setFieldDirection(FieldDirection.LEFT);
22+
} else VISION.setFieldDirection(FieldDirection.RIGHT);
23+
}
24+
}

‎src/main/java/frc/team2767/deepspace/command/approach/CalculateRotationCommand.java‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
package frc.team2767.deepspace.command.approach;
2+
3+
import edu.wpi.first.wpilibj.command.InstantCommand;
4+
import frc.team2767.deepspace.Robot;
5+
import frc.team2767.deepspace.subsystem.DriveSubsystem;
6+
import frc.team2767.deepspace.subsystem.VisionSubsystem;
7+
import frc.team2767.deepspace.util.RotationCalculator;
8+
import org.slf4j.Logger;
9+
import org.slf4j.LoggerFactory;
10+
11+
public class CalculateRotationCommand extends InstantCommand {
12+
13+
private static final VisionSubsystem VISION = Robot.VISION;
14+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
15+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
16+
17+
public CalculateRotationCommand() {}
18+
19+
@Override
20+
protected void initialize() {
21+
RotationCalculator rotationCalculator =
22+
new RotationCalculator(
23+
VISION.getRawBearing(),
24+
VISION.getRawRange(),
25+
VISION.getCameraPositionBearing(),
26+
Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360));
27+
28+
VISION.setCorrectedHeading(rotationCalculator.getHeading());
29+
30+
logger.debug("heading = {}", VISION.getCorrectedHeading());
31+
}
32+
}

‎src/main/java/frc/team2767/deepspace/command/approach/CalculateTwistCommand.java‎

Lines changed: 32 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -3,43 +3,64 @@
33
import edu.wpi.first.wpilibj.command.InstantCommand;
44
import frc.team2767.deepspace.Robot;
55
import frc.team2767.deepspace.subsystem.DriveSubsystem;
6-
import frc.team2767.deepspace.subsystem.ElevatorSubsystem;
76
import frc.team2767.deepspace.subsystem.VisionSubsystem;
87
import frc.team2767.deepspace.util.TwistCalculator;
98
import org.slf4j.Logger;
109
import org.slf4j.LoggerFactory;
1110

1211
public class CalculateTwistCommand extends InstantCommand {
1312

14-
private static final Logger logger = LoggerFactory.getLogger(ElevatorSubsystem.class);
1513
private static final VisionSubsystem VISION = Robot.VISION;
1614
private static final DriveSubsystem DRIVE = Robot.DRIVE;
15+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
16+
private double offset;
1717

18-
public CalculateTwistCommand() {
19-
requires(DRIVE);
20-
requires(VISION);
18+
public CalculateTwistCommand(double offset) {
19+
this.offset = offset;
2120
}
2221

2322
@Override
2423
protected void initialize() {
25-
double targetYaw = 0.0; // VISION.getTargetYaw(); // FIXME
24+
double targetYaw = -90.0;
25+
double cameraPositionBearing = VISION.getCameraPositionBearing();
26+
27+
double yaw = Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360);
28+
2629
logger.info(
2730
"pyeye bearing={} range={} current gyro = {}",
2831
VISION.getRawBearing(),
2932
VISION.getRawRange(),
30-
DRIVE.getGyro().getAngle());
33+
yaw);
3134

3235
TwistCalculator twistCalculator =
3336
new TwistCalculator(
3437
VISION.getRawBearing(),
3538
VISION.getRawRange(),
3639
VISION.getCameraX(),
3740
VISION.getCameraY(),
38-
VISION.getCameraPositionBearing(),
39-
DRIVE.getGyro().getAngle(),
41+
cameraPositionBearing,
42+
yaw,
4043
targetYaw);
4144

42-
VISION.setCorrectedHeading(-180);
43-
VISION.setCorrectedRange(50.0);
45+
double offsetX = offset * Math.cos(Math.toRadians(targetYaw + cameraPositionBearing));
46+
double offsetY = offset * Math.sin(Math.toRadians(targetYaw + cameraPositionBearing));
47+
48+
double[] corrected = twistCalculator.getXYCorrected();
49+
50+
double newX = corrected[0] - offsetX;
51+
double newY = corrected[1] - offsetY;
52+
53+
logger.debug("x={} y={}", newX, newY);
54+
55+
double finalRange = Math.hypot(newX, newY);
56+
double finalHeading = Math.toDegrees(Math.atan2(newY, newX));
57+
58+
VISION.setCorrectedHeading(finalHeading);
59+
VISION.setCorrectedRange(finalRange);
60+
61+
logger.debug(
62+
"corrected heading = {} distance = {}",
63+
VISION.getCorrectedHeading(),
64+
VISION.getCorrectedRange());
4465
}
4566
}

‎src/main/java/frc/team2767/deepspace/command/approach/DriveTwistCommand.java‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,6 @@ public class DriveTwistCommand extends Command {
1515
private double targetYaw;
1616
private double heading;
1717

18-
public DriveTwistCommand(double heading, double inches) {
19-
this(heading, inches, Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360));
20-
}
21-
2218
public DriveTwistCommand(double heading, double inches, double targetYaw) {
2319

2420
this.heading = heading;

‎src/main/java/frc/team2767/deepspace/command/approach/FieldSquarePickupAlignmentCommand.java‎

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -3,34 +3,29 @@
33
import edu.wpi.first.wpilibj.command.CommandGroup;
44
import edu.wpi.first.wpilibj.command.ConditionalCommand;
55
import frc.team2767.deepspace.Robot;
6-
import frc.team2767.deepspace.command.YawCommand;
76
import frc.team2767.deepspace.command.log.LogCommand;
87
import frc.team2767.deepspace.subsystem.DriveSubsystem;
98
import frc.team2767.deepspace.subsystem.FieldDirection;
109
import frc.team2767.deepspace.subsystem.VisionSubsystem;
11-
import org.slf4j.Logger;
12-
import org.slf4j.LoggerFactory;
1310

1411
public class FieldSquarePickupAlignmentCommand extends ConditionalCommand {
1512

1613
private static final VisionSubsystem VISION = Robot.VISION;
1714
private static final DriveSubsystem DRIVE = Robot.DRIVE;
1815

19-
private final Logger logger = LoggerFactory.getLogger(this.getClass());
20-
2116
public FieldSquarePickupAlignmentCommand() {
2217
super(
2318
new CommandGroup() {
2419
{
2520
addSequential(new LogCommand("Field left"));
26-
addSequential(new YawCommand(-90.0));
21+
addSequential(new YawToTargetCommand(-90.0));
2722
addSequential(new SetTargetYawCommand(-90.0));
2823
}
2924
},
3025
new CommandGroup() {
3126
{
3227
addSequential(new LogCommand("Field right"));
33-
addSequential(new YawCommand(90.0));
28+
addSequential(new YawToTargetCommand(90.0));
3429
addSequential(new SetTargetYawCommand(90.0));
3530
}
3631
});

‎src/main/java/frc/team2767/deepspace/command/approach/OpenLoopDriveUntilCurrentCommand.java‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
package frc.team2767.deepspace.command.approach;
2+
3+
import edu.wpi.first.wpilibj.Timer;
4+
import edu.wpi.first.wpilibj.command.Command;
5+
import frc.team2767.deepspace.Robot;
6+
import frc.team2767.deepspace.subsystem.DriveSubsystem;
7+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
8+
import org.slf4j.Logger;
9+
import org.slf4j.LoggerFactory;
10+
11+
public class OpenLoopDriveUntilCurrentCommand extends Command {
12+
13+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
14+
private static final VacuumSubsystem VACUUM = Robot.VACUUM;
15+
private static final double TRANSITION_CURRENT_DIFFERENCE = 50.0;
16+
private static final double DIRECTION = -0.25;
17+
private static final double OUT_DRIVE_SECONDS = 0.5;
18+
private static final double SOLENOID_DELAY = 0.2;
19+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
20+
private DriveState driveState;
21+
private double initialCurrent;
22+
private double outDriveInitTime;
23+
private double solenoidDelayInit;
24+
25+
public OpenLoopDriveUntilCurrentCommand() {
26+
setTimeout(5.0);
27+
requires(DRIVE);
28+
}
29+
30+
@Override
31+
protected void initialize() {
32+
driveState = DriveState.FAST;
33+
initialCurrent = DRIVE.getAverageOutputCurrent();
34+
}
35+
36+
@Override
37+
protected void execute() {
38+
switch (driveState) {
39+
case FAST:
40+
DRIVE.setWheels(DIRECTION, DriveState.FAST.velocity);
41+
double averageCurrent = DRIVE.getAverageOutputCurrent();
42+
if (averageCurrent > initialCurrent + TRANSITION_CURRENT_DIFFERENCE) {
43+
driveState = DriveState.CLOSE_SOLENOID;
44+
logger.debug("set state to {}, current = {}", driveState, averageCurrent);
45+
}
46+
break;
47+
case CLOSE_SOLENOID:
48+
DRIVE.setWheels(DIRECTION, DriveState.CLOSE_SOLENOID.velocity);
49+
VACUUM.setSolenoidsState(VacuumSubsystem.SolenoidStates.PRESSURE_ACCUMULATE);
50+
solenoidDelayInit = Timer.getFPGATimestamp();
51+
driveState = DriveState.WAIT;
52+
case WAIT:
53+
if (Timer.getFPGATimestamp() - solenoidDelayInit > SOLENOID_DELAY) {
54+
driveState = DriveState.OUT;
55+
outDriveInitTime = Timer.getFPGATimestamp();
56+
logger.debug("set state to {}", driveState);
57+
}
58+
break;
59+
case OUT:
60+
DRIVE.setWheels(DIRECTION, DriveState.OUT.velocity);
61+
if (Timer.getFPGATimestamp() - outDriveInitTime > OUT_DRIVE_SECONDS) {
62+
driveState = DriveState.DONE;
63+
logger.debug("set state to {}", driveState);
64+
}
65+
break;
66+
case DONE:
67+
break;
68+
}
69+
}
70+
71+
@Override
72+
protected boolean isFinished() {
73+
return false;
74+
}
75+
76+
private enum DriveState {
77+
FAST(0.5),
78+
CLOSE_SOLENOID(0.06),
79+
WAIT(0.06),
80+
OUT(-0.6),
81+
DONE(0.0);
82+
83+
private double velocity;
84+
85+
DriveState(double velocity) {
86+
this.velocity = velocity;
87+
}
88+
}
89+
}

‎src/main/java/frc/team2767/deepspace/command/approach/OpenLoopDriveUntilSuctionCommand.java‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
package frc.team2767.deepspace.command.approach;
2+
3+
import edu.wpi.first.wpilibj.Timer;
4+
import edu.wpi.first.wpilibj.command.Command;
5+
import frc.team2767.deepspace.Robot;
6+
import frc.team2767.deepspace.subsystem.DriveSubsystem;
7+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
8+
import org.slf4j.Logger;
9+
import org.slf4j.LoggerFactory;
10+
import org.strykeforce.thirdcoast.swerve.SwerveDrive;
11+
12+
public class OpenLoopDriveUntilSuctionCommand extends Command {
13+
14+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
15+
private static final VacuumSubsystem VACUUM = Robot.VACUUM;
16+
private static final double TRANSITION_PRESSURE_DIFFERENCE = 3.0;
17+
private static final double HATCH_SEAL_GOOD_ENOUGH = 10.0;
18+
private static final double OUT_DRIVE_SECONDS = 0.25;
19+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
20+
private double direction;
21+
private double initialPressure;
22+
private DriveState driveState;
23+
private double outDriveInitTime;
24+
25+
public OpenLoopDriveUntilSuctionCommand() {
26+
setTimeout(5.0);
27+
requires(DRIVE);
28+
}
29+
30+
@Override
31+
protected void initialize() {
32+
driveState = DriveState.FAST;
33+
if (HATCH_SEAL_GOOD_ENOUGH < TRANSITION_PRESSURE_DIFFERENCE) {
34+
logger.warn("Transition pressures not set correctly");
35+
}
36+
37+
initialPressure = VACUUM.getPressure();
38+
logger.debug("init pressure = {}", initialPressure);
39+
DRIVE.setDriveMode(SwerveDrive.DriveMode.OPEN_LOOP);
40+
direction = 0.25;
41+
42+
if (Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360) < 0) {
43+
direction *= -1;
44+
}
45+
}
46+
47+
@Override
48+
protected void execute() {
49+
switch (driveState) {
50+
case FAST:
51+
DRIVE.setWheels(direction, DriveState.FAST.velocity);
52+
double currentFastPressure = VACUUM.getPressure();
53+
if (currentFastPressure - initialPressure > TRANSITION_PRESSURE_DIFFERENCE) {
54+
logger.debug("current pressure = {}", currentFastPressure);
55+
driveState = DriveState.SLOW;
56+
}
57+
break;
58+
59+
case SLOW:
60+
DRIVE.setWheels(direction, DriveState.SLOW.velocity);
61+
double currentSlowPressure = VACUUM.getPressure();
62+
if (currentSlowPressure - initialPressure > HATCH_SEAL_GOOD_ENOUGH) {
63+
logger.debug("pressure reached: current pressure = {}", currentSlowPressure);
64+
outDriveInitTime = Timer.getFPGATimestamp();
65+
driveState = DriveState.OUT;
66+
}
67+
break;
68+
69+
case OUT:
70+
DRIVE.setWheels(direction, DriveState.OUT.velocity);
71+
if (Timer.getFPGATimestamp() - outDriveInitTime > OUT_DRIVE_SECONDS) {
72+
driveState = DriveState.DONE;
73+
}
74+
}
75+
}
76+
77+
@Override
78+
protected boolean isFinished() {
79+
return driveState == DriveState.DONE;
80+
}
81+
82+
@Override
83+
protected void end() {
84+
if (isTimedOut()) {
85+
logger.info("Timed Out");
86+
}
87+
}
88+
89+
private enum DriveState {
90+
FAST(0.2),
91+
SLOW(0.06),
92+
OUT(-0.9),
93+
DONE(0.0);
94+
95+
private double velocity;
96+
97+
DriveState(double velocity) {
98+
this.velocity = velocity;
99+
}
100+
}
101+
}

0 commit comments

Comments
 (0)