Skip to content

Commit 7b3d6a3

Browse files
aaditsangvikaraaditsangvikar
authored
Merge pull request #59 from TotalTaxAmount/dev
Final changes
2 parents c7f12dc + cf928f4 commit 7b3d6a3

File tree

6 files changed

+62
-69
lines changed

6 files changed

+62
-69
lines changed

‎src/main/java/org/blackknights/RobotContainer.java‎

Lines changed: 16 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -88,17 +88,16 @@ private void configureBindings() {
8888
true,
8989
true));
9090

91-
primaryController.rightBumper.whileTrue(
91+
primaryController.leftBumper.whileTrue(
9292
getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext()));
9393

94-
primaryController.leftBumper.whileTrue(
94+
primaryController.rightBumper.whileTrue(
9595
new ParallelCommandGroup(
9696
new ElevatorArmCommand(
9797
elevatorSubsystem,
9898
armSubsystem,
9999
() -> ScoringConstants.ScoringHeights.INTAKE),
100-
new IntakeCommand(
101-
intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false)));
100+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)));
102101

103102
elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem));
104103

@@ -116,9 +115,7 @@ private void configureBindings() {
116115

117116
secondaryController.aButton.whileTrue(
118117
new ElevatorArmCommand(
119-
elevatorSubsystem,
120-
armSubsystem,
121-
() -> ScoringConstants.ScoringHeights.INTAKE));
118+
elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L1));
122119

123120
secondaryController.bButton.whileTrue(
124121
new ElevatorArmCommand(
@@ -132,11 +129,16 @@ private void configureBindings() {
132129
new ElevatorArmCommand(
133130
elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L4));
134131

135-
secondaryController.leftBumper.whileTrue(
136-
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false));
132+
secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards()));
137133

138134
secondaryController.rightBumper.whileTrue(
139-
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE, () -> false));
135+
new InstantCommand(() -> coralQueue.stepBackwards()));
136+
137+
// secondaryController.rightTrigger.whileTrue(
138+
// new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE));
139+
//
140+
// secondaryController.leftTrigger.whileTrue(
141+
// new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
140142
}
141143

142144
/** Runs once when the code starts */
@@ -153,7 +155,8 @@ public void robotPeriodic() {
153155

154156
/** Runs ones when enabled in teleop */
155157
public void teleopInit() {
156-
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
158+
if (cqProfiles.getSelected() != null)
159+
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
157160
}
158161

159162
/**
@@ -206,10 +209,7 @@ private Command getPlaceCommand(
206209
() -> currentSupplier.get().getPose(),
207210
true,
208211
"fine"),
209-
new IntakeCommand(
210-
intakeSubsystem,
211-
IntakeCommand.IntakeMode.OUTTAKE,
212-
() -> armSubsystem.hasGamePiece())
212+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
213213
.withTimeout(2)),
214214
new ElevatorArmCommand(
215215
elevatorSubsystem,
@@ -245,10 +245,7 @@ private Command getAutoIntakeCommand() {
245245
: ScoringConstants.INTAKE_RED,
246246
true,
247247
"rough"),
248-
new IntakeCommand(
249-
intakeSubsystem,
250-
IntakeCommand.IntakeMode.INTAKE,
251-
() -> armSubsystem.hasGamePiece())
248+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)
252249
.withTimeout(2)),
253250
new ElevatorArmCommand(
254251
elevatorSubsystem,

‎src/main/java/org/blackknights/commands/IntakeCommand.java‎

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2,28 +2,23 @@
22
package org.blackknights.commands;
33

44
import edu.wpi.first.wpilibj2.command.Command;
5-
import java.util.function.BooleanSupplier;
65
import org.blackknights.subsystems.IntakeSubsystem;
76
import org.blackknights.utils.ConfigManager;
87

98
/** Command to intake and outtake */
109
public class IntakeCommand extends Command {
1110
private final IntakeSubsystem intakeSubsystem;
1211
private final IntakeMode mode;
13-
private final BooleanSupplier hasGamePiece;
1412

1513
/**
1614
* Create a new intake command
1715
*
1816
* @param intakeSubsystem The instance of {@link IntakeSubsystem}
1917
* @param mode The intake mode ({@link IntakeMode})
20-
* @param hasGamePiece A {@link BooleanSupplier} supplying if we currently have a coral
2118
*/
22-
public IntakeCommand(
23-
IntakeSubsystem intakeSubsystem, IntakeMode mode, BooleanSupplier hasGamePiece) {
19+
public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) {
2420
this.intakeSubsystem = intakeSubsystem;
2521
this.mode = mode;
26-
this.hasGamePiece = hasGamePiece;
2722
addRequirements(intakeSubsystem);
2823
}
2924

@@ -50,10 +45,10 @@ public void end(boolean interrupted) {
5045
intakeSubsystem.setVoltage(0);
5146
}
5247

53-
// @Override
54-
// public boolean isFinished() {
55-
// return mode.equals(IntakeMode.INTAKE) && hasGamePiece.getAsBoolean();
56-
// }
48+
@Override
49+
public boolean isFinished() {
50+
return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak();
51+
}
5752

5853
/** Enum of the different intake modes */
5954
public enum IntakeMode {

‎src/main/java/org/blackknights/constants/ArmConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ public class ArmConstants {
1414
public static final TrapezoidProfile.Constraints PIVOT_CONSTRAINTS =
1515
new TrapezoidProfile.Constraints(PIVOT_MAX_VELOCITY, PIVOT_MAX_ACCELERATION);
1616

17-
public static final double PIVOT_ENCODER_OFFSET = 1.581; // 5.167
17+
public static final double PIVOT_ENCODER_OFFSET = 1.581 - 0.092; // 5.167
1818

1919
public static final double PIVOT_KS = 0.0;
2020
public static final double PIVOT_KG = 0.0;

‎src/main/java/org/blackknights/subsystems/ArmSubsystem.java‎

Lines changed: 32 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,17 @@
44
import com.revrobotics.AbsoluteEncoder;
55
import com.revrobotics.spark.SparkBase;
66
import com.revrobotics.spark.SparkFlex;
7-
import com.revrobotics.spark.SparkLimitSwitch;
87
import com.revrobotics.spark.SparkLowLevel;
98
import com.revrobotics.spark.config.SparkBaseConfig;
109
import com.revrobotics.spark.config.SparkFlexConfig;
1110
import edu.wpi.first.math.MathUtil;
1211
import edu.wpi.first.math.controller.ArmFeedforward;
1312
import edu.wpi.first.math.controller.ProfiledPIDController;
14-
import edu.wpi.first.networktables.DoubleEntry;
15-
import edu.wpi.first.networktables.NetworkTableInstance;
13+
import edu.wpi.first.math.trajectory.TrapezoidProfile;
1614
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1715
import org.blackknights.constants.ArmConstants;
1816
import org.blackknights.utils.ConfigManager;
17+
import org.blackknights.utils.NetworkTablesUtils;
1918

2019
/** Subsystem for controlling arm */
2120
public class ArmSubsystem extends SubsystemBase {
@@ -24,19 +23,7 @@ public class ArmSubsystem extends SubsystemBase {
2423

2524
private final AbsoluteEncoder pivotAbsEncoder = pivotMotor.getAbsoluteEncoder();
2625

27-
private final SparkLimitSwitch armLinebreak;
28-
29-
private final DoubleEntry armEncoderPos =
30-
NetworkTableInstance.getDefault()
31-
.getTable("Arm")
32-
.getDoubleTopic("EncoderPos")
33-
.getEntry(getPivotAngle());
34-
35-
private final DoubleEntry armEncoderVelocity =
36-
NetworkTableInstance.getDefault()
37-
.getTable("Arm")
38-
.getDoubleTopic("EncoderVelocity")
39-
.getEntry(getPivotSpeed());
26+
private final NetworkTablesUtils NTDebug = NetworkTablesUtils.getTable("debug");
4027

4128
private final ProfiledPIDController pivotPID =
4229
new ProfiledPIDController(
@@ -82,8 +69,6 @@ public ArmSubsystem() {
8269
pivotConfig,
8370
SparkBase.ResetMode.kResetSafeParameters,
8471
SparkBase.PersistMode.kPersistParameters);
85-
86-
armLinebreak = pivotMotor.getForwardLimitSwitch();
8772
}
8873

8974
/**
@@ -95,15 +80,6 @@ public void setPivotSpeed(double speed) {
9580
pivotMotor.set(speed);
9681
}
9782

98-
/**
99-
* Checks if the hand has a game piece
100-
*
101-
* @return returns if the hand has a game piece
102-
*/
103-
public boolean hasGamePiece() {
104-
return armLinebreak.isPressed();
105-
}
106-
10783
/**
10884
* moves arm to a set angle
10985
*
@@ -115,6 +91,9 @@ public void setPivotAngle(double angle) {
11591
double pidValue = pivotPID.calculate(getPivotAngle(), angle);
11692
double ffValue = pivotFF.calculate(angle, 0);
11793

94+
NTDebug.setEntry("Arm ff out", ffValue);
95+
NTDebug.setEntry("Arm pid out", ffValue);
96+
11897
setPivotSpeed(pidValue + ffValue);
11998
}
12099

@@ -128,16 +107,39 @@ public double getPivotAngle() {
128107
// ? pivotAbsEncoder.getPosition() - 2 * Math.PI -
129108
// ArmConstants.PIVOT_ENCODER_OFFSET
130109
// : pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET;
131-
return Math.PI * 2 - pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET;
110+
return Math.PI * 2
111+
- pivotAbsEncoder.getPosition()
112+
- ConfigManager.getInstance()
113+
.get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET);
132114
}
133115

134116
public double getPivotSpeed() {
135117
return pivotAbsEncoder.getVelocity();
136118
}
137119

138120
public void periodic() {
139-
armEncoderPos.set(getPivotAngle());
140-
armEncoderVelocity.set(getPivotSpeed());
121+
NTDebug.setEntry("Arm Encoder Pos", getPivotAngle());
122+
NTDebug.setEntry("Arm Encoder Speed", getPivotSpeed());
123+
124+
NTDebug.setEntry("Arm PID Error", pivotPID.getPositionError());
125+
NTDebug.setEntry("Arm PID Setpoint", pivotPID.getGoal().position);
126+
127+
pivotPID.setConstraints(
128+
new TrapezoidProfile.Constraints(
129+
Math.toRadians(
130+
ConfigManager.getInstance()
131+
.get(
132+
"arm_max_vel_degs",
133+
Math.toDegrees(ArmConstants.PIVOT_MAX_VELOCITY))),
134+
ConfigManager.getInstance()
135+
.get(
136+
"arm_max_accel_degs",
137+
Math.toDegrees(ArmConstants.PIVOT_MAX_ACCELERATION))));
138+
139+
pivotPID.setTolerance(
140+
Math.toRadians(
141+
ConfigManager.getInstance().get("arm_tol", ArmConstants.PIVOT_TOLERANCE)));
142+
141143
pivotPID.setP(ConfigManager.getInstance().get("arm_p", ArmConstants.PIVOT_P));
142144
pivotPID.setI(ConfigManager.getInstance().get("arm_i", ArmConstants.PIVOT_I));
143145
pivotPID.setD(ConfigManager.getInstance().get("arm_d", ArmConstants.PIVOT_D));

‎src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java‎

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
import edu.wpi.first.math.trajectory.TrapezoidProfile;
1313
import edu.wpi.first.networktables.DoubleEntry;
1414
import edu.wpi.first.networktables.NetworkTableInstance;
15-
import edu.wpi.first.wpilibj.DigitalInput;
1615
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1716
import org.blackknights.constants.ElevatorConstants;
1817
import org.blackknights.utils.ConfigManager;
@@ -32,9 +31,6 @@ public class ElevatorSubsystem extends SubsystemBase {
3231
private final RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder();
3332

3433
// Linebreaks
35-
private final DigitalInput topLineBreak = new DigitalInput(ElevatorConstants.TOP_LINEBREAK_ID);
36-
private final DigitalInput bottomLineBreak =
37-
new DigitalInput(ElevatorConstants.BOTTOM_LINEBREAK_ID);
3834

3935
private final ProfiledPIDController elevatorPID =
4036
new ProfiledPIDController(
@@ -210,10 +206,6 @@ public double getElevatorVelocity() {
210206
return encoderAverageVel * ElevatorConstants.ROTATIONS_TO_METERS;
211207
}
212208

213-
public boolean getBottomLinebreak() {
214-
return !bottomLineBreak.get();
215-
}
216-
217209
/**
218210
* Check if elevator is at target position
219211
*

‎src/main/java/org/blackknights/subsystems/IntakeSubsystem.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,16 +2,19 @@
22
package org.blackknights.subsystems;
33

44
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
5+
import edu.wpi.first.wpilibj.DigitalInput;
56
import edu.wpi.first.wpilibj2.command.SubsystemBase;
67
import org.blackknights.constants.ArmConstants;
78

89
/** Subsystem to manage the intake (NOT HAND) */
910
public class IntakeSubsystem extends SubsystemBase {
1011
private final WPI_TalonSRX motor = new WPI_TalonSRX(ArmConstants.MOTOR_ID);
1112

13+
private final DigitalInput intakeLinebreak = new DigitalInput(0);
14+
1215
/** Create a new intake subsystem */
1316
public IntakeSubsystem() {
14-
motor.setInverted(false);
17+
motor.setInverted(true);
1518
motor.enableCurrentLimit(true);
1619
motor.configContinuousCurrentLimit(20);
1720
motor.configPeakCurrentLimit(0);
@@ -34,4 +37,8 @@ public void setSpeed(double speed) {
3437
public void setVoltage(double voltage) {
3538
motor.setVoltage(voltage);
3639
}
40+
41+
public boolean getLinebreak() {
42+
return !intakeLinebreak.get();
43+
}
3744
}

0 commit comments

Comments
 (0)