Skip to content

Commit 3655202

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #105 from strykeforce/iri-hotfix
Iri hotfix
2 parents 6011063 + a293736 commit 3655202

19 files changed

+188
-47
lines changed

‎src/main/deploy/paths/MiddleNote4_NonAmpShoot2_B.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
max_velocity = 3.0 #m/s
2-
max_acceleration = 3.0 #m/s
2+
max_acceleration = 1.5 #m/s
33
start_velocity = 0.0 #m/s
44
end_velocity = 0.0 #m/s
55
is_reversed = false

‎src/main/deploy/paths/MiddleNote5_NonAmpShoot2.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
max_velocity = 5.0 #m/s
2-
max_acceleration = 3.0 #m/s
2+
max_acceleration = 1.5 #m/s
33
start_velocity = 0.0 #m/s
44
end_velocity = 0.0 #m/s
55
is_reversed = false

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,6 @@
6666
import frc.robot.commands.robotState.ClimbCommand;
6767
import frc.robot.commands.robotState.ClimbTrapDecendCommand;
6868
import frc.robot.commands.robotState.DecendCommand;
69-
import frc.robot.commands.robotState.FeedCommand;
7069
import frc.robot.commands.robotState.FullTrapClimbCommand;
7170
import frc.robot.commands.robotState.IntakeCommand;
7271
import frc.robot.commands.robotState.MovingVisionShootCommand;
@@ -80,6 +79,7 @@
8079
import frc.robot.commands.robotState.SpeedUpPassCommand;
8180
import frc.robot.commands.robotState.StowCommand;
8281
import frc.robot.commands.robotState.SubWooferCommand;
82+
import frc.robot.commands.robotState.TogglePunchAirCommand;
8383
import frc.robot.commands.robotState.TunedShotCommand;
8484
import frc.robot.commands.robotState.TuningOffCommand;
8585
import frc.robot.commands.robotState.TuningShootCommand;
@@ -956,10 +956,10 @@ private void configureOperatorBindings() {
956956
.onTrue(new SpeedUpPassCommand(robotStateSubsystem, superStructure));
957957

958958
// Defense
959-
// new JoystickButton(xboxController, XboxController.Button.kB.value)
960-
// .onTrue(new TogglePunchAirCommand(robotStateSubsystem));
959+
new JoystickButton(xboxController, XboxController.Button.kB.value)
960+
.onTrue(new TogglePunchAirCommand(robotStateSubsystem));
961961

962-
new JoystickButton(xboxController, XboxController.Button.kB.value).onTrue(testAuto);
962+
// new JoystickButton(xboxController, XboxController.Button.kB.value).onTrue(testAuto);
963963

964964
// new JoystickButton(xboxController, XboxController.Button.kB.value)
965965
// .onTrue(new ToggleDefenseCommand(robotStateSubsystem, superStructure,
@@ -1088,18 +1088,18 @@ private void configureDriverBindings() {
10881088
new JoystickButton(driveJoystick, Button.SWG_DWN.id)
10891089
.onFalse(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
10901090

1091-
new JoystickButton(driveJoystick, Button.SWF_UP.id)
1092-
.onFalse(
1093-
new FeedCommand(
1094-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
1095-
new JoystickButton(driveJoystick, Button.SWF_DWN.id)
1096-
.onTrue(
1097-
new FeedCommand(
1098-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
1099-
new JoystickButton(driveJoystick, Button.SWF_DWN.id)
1100-
.onFalse(
1101-
new FeedCommand(
1102-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
1091+
// new JoystickButton(driveJoystick, Button.SWF_UP.id)
1092+
// .onFalse(
1093+
// new FeedCommand(
1094+
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
1095+
// new JoystickButton(driveJoystick, Button.SWF_DWN.id)
1096+
// .onTrue(
1097+
// new FeedCommand(
1098+
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
1099+
// new JoystickButton(driveJoystick, Button.SWF_DWN.id)
1100+
// .onFalse(
1101+
// new FeedCommand(
1102+
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
11031103
}
11041104

11051105
public void configureClimbTestBindings() {

‎src/main/java/frc/robot/commands/auton/DeadeyeHuntCommand.java‎

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import edu.wpi.first.math.controller.PIDController;
44
import edu.wpi.first.math.controller.ProfiledPIDController;
55
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
6+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
67
import edu.wpi.first.wpilibj2.command.Command;
78
import frc.robot.constants.AutonConstants;
89
import frc.robot.subsystems.drive.DriveSubsystem;
@@ -56,7 +57,13 @@ public void initialize() {
5657
driveSubsystem.move(AutonConstants.kXSpeed / (xSpeed * xSpeed + 1), ySpeed, 0.0, false);
5758
seenNote++;
5859
} else {
59-
driveSubsystem.move(0.0, 0.0, AutonConstants.kDeadeyeHuntOmegaRadps, false);
60+
driveSubsystem.move(
61+
0.0,
62+
0.0,
63+
robotStateSubsystem.getAllianceColor() == Alliance.Blue
64+
? AutonConstants.kDeadeyeHuntOmegaRadps
65+
: -AutonConstants.kDeadeyeHuntOmegaRadps,
66+
false);
6067
}
6168
}
6269

@@ -70,7 +77,13 @@ public void execute() {
7077
huntState = HuntState.DRIVING;
7178
}
7279
} else {
73-
driveSubsystem.move(0.0, 0.0, AutonConstants.kDeadeyeHuntOmegaRadps, false);
80+
driveSubsystem.move(
81+
0.0,
82+
0.0,
83+
robotStateSubsystem.getAllianceColor() == Alliance.Blue
84+
? AutonConstants.kDeadeyeHuntOmegaRadps
85+
: -AutonConstants.kDeadeyeHuntOmegaRadps,
86+
false);
7487
seenNote = 0;
7588
}
7689
break;

‎src/main/java/frc/robot/commands/auton/DoNothingCommand.java‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,12 @@
11
package frc.robot.commands.auton;
22

3+
import edu.wpi.first.math.geometry.Pose2d;
34
import edu.wpi.first.wpilibj.DriverStation.Alliance;
45
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
56
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
67
import frc.robot.commands.drive.ResetGyroCommand;
8+
import frc.robot.commands.drive.ResetOdomCommand;
9+
import frc.robot.commands.drive.setAngleOffsetCommand;
710
import frc.robot.commands.elbow.ZeroElbowCommand;
811
import frc.robot.commands.robotState.SubWooferCommand;
912
import frc.robot.subsystems.auto.AutoCommandInterface;
@@ -28,7 +31,8 @@ public DoNothingCommand(
2831
DriveSubsystem driveSubsystem,
2932
SuperStructure superStructure,
3033
MagazineSubsystem magazineSubsystem,
31-
ElbowSubsystem elbowSubsystem) {
34+
ElbowSubsystem elbowSubsystem,
35+
Pose2d start) {
3236
this.robotStateSubsystem = robotStateSubsystem;
3337
this.driveSubsystem = driveSubsystem;
3438
this.superStructure = superStructure;
@@ -39,6 +43,8 @@ public DoNothingCommand(
3943
new ParallelCommandGroup(
4044
new ZeroElbowCommand(elbowSubsystem), // zero elbow
4145
new ResetGyroCommand(driveSubsystem)), // reset gyro
46+
new setAngleOffsetCommand(driveSubsystem, start.getRotation().getDegrees()),
47+
new ResetOdomCommand(driveSubsystem, start),
4248
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem)); // shoot
4349
}
4450

‎src/main/java/frc/robot/commands/auton/MiddleNote3AndWingNotesCommand.java‎

Lines changed: 24 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,22 @@
11
package frc.robot.commands.auton;
22

3+
import edu.wpi.first.math.geometry.Rotation2d;
34
import edu.wpi.first.wpilibj.DriverStation.Alliance;
45
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
56
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
67
import edu.wpi.first.wpilibj2.command.WaitCommand;
78
import frc.robot.commands.drive.DriveAutonCommand;
89
import frc.robot.commands.drive.ResetGyroCommand;
10+
import frc.robot.commands.drive.TurnUntilAngleCommand;
911
import frc.robot.commands.drive.setAngleOffsetCommand;
1012
import frc.robot.commands.elbow.ZeroElbowCommand;
1113
import frc.robot.commands.robotState.IgnoreNotesCommand;
1214
import frc.robot.commands.robotState.IntakeCommand;
13-
import frc.robot.commands.robotState.PrepShooterCommand;
1415
import frc.robot.commands.robotState.SubWooferCommand;
1516
import frc.robot.commands.robotState.VisionShootCommand;
17+
import frc.robot.commands.superStructure.SpinUpWheelsCommand;
1618
import frc.robot.constants.AutonConstants;
19+
import frc.robot.constants.SuperStructureConstants;
1720
import frc.robot.subsystems.auto.AutoCommandInterface;
1821
import frc.robot.subsystems.drive.DriveSubsystem;
1922
import frc.robot.subsystems.elbow.ElbowSubsystem;
@@ -99,24 +102,36 @@ public MiddleNote3AndWingNotesCommand(
99102
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem))),
100103
new ParallelCommandGroup(
101104
middleNote3MiddleShoot3,
102-
new SequentialCommandGroup(
103-
new AutoWaitNoteStagedCommand(robotStateSubsystem),
104-
new PrepShooterCommand(
105-
superStructure, robotStateSubsystem, AutonConstants.Setpoints.MS3))),
105+
new AutoWaitNoteStagedCommand(robotStateSubsystem),
106+
new SpinUpWheelsCommand(
107+
superStructure,
108+
SuperStructureConstants.kShooterSubwooferSetPoint,
109+
SuperStructureConstants.kShooterSubwooferSetPoint)),
106110
new WaitCommand(0.02),
107111
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
108112
middleShoot3WingNote3,
109113
new ParallelCommandGroup(
110114
wingNote3MidInit,
111-
new SequentialCommandGroup(
112-
new AutoWaitNoteStagedCommand(robotStateSubsystem),
113-
new PrepShooterCommand(
114-
superStructure, robotStateSubsystem, AutonConstants.Setpoints.MI1))),
115+
new AutoWaitNoteStagedCommand(robotStateSubsystem),
116+
new SpinUpWheelsCommand(
117+
superStructure,
118+
SuperStructureConstants.kShooterSubwooferSetPoint,
119+
SuperStructureConstants.kShooterSubwooferSetPoint)),
115120
new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
116121
midInitWingNote1,
117122
new AutoWaitNoteStagedCommand(robotStateSubsystem),
118123
new VisionShootCommand(
119124
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem),
125+
new TurnUntilAngleCommand(
126+
driveSubsystem,
127+
robotStateSubsystem,
128+
Rotation2d.fromDegrees(
129+
robotStateSubsystem.getAllianceColor() == Alliance.Blue
130+
? AutonConstants.kDeadeyeHuntStartYawDegs
131+
: 180 - AutonConstants.kDeadeyeHuntStartYawDegs),
132+
robotStateSubsystem.getAllianceColor() == Alliance.Blue
133+
? AutonConstants.kDeadeyeHuntOmegaRadps
134+
: -AutonConstants.kDeadeyeHuntOmegaRadps),
120135
new DeadeyeHuntCommand(deadeye, driveSubsystem, robotStateSubsystem, ledSubsystem),
121136
new AutoWaitNoteStagedCommand(robotStateSubsystem),
122137
new VisionShootCommand(

‎src/main/java/frc/robot/commands/drive/ResetOdomCommand.java‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
package frc.robot.commands.drive;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.wpilibj2.command.InstantCommand;
5+
import frc.robot.subsystems.drive.DriveSubsystem;
6+
7+
public class ResetOdomCommand extends InstantCommand {
8+
private DriveSubsystem driveSubsystem;
9+
private Pose2d pose;
10+
11+
public ResetOdomCommand(DriveSubsystem driveSubsystem, Pose2d pose) {
12+
this.driveSubsystem = driveSubsystem;
13+
this.pose = pose;
14+
addRequirements(driveSubsystem);
15+
}
16+
17+
@Override
18+
public void initialize() {
19+
driveSubsystem.resetOdometry(pose);
20+
}
21+
}

‎src/main/java/frc/robot/commands/drive/TurnUntilAngleCommand.java‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
package frc.robot.commands.drive;
2+
3+
import edu.wpi.first.math.geometry.Rotation2d;
4+
import edu.wpi.first.math.util.Units;
5+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
import frc.robot.subsystems.drive.DriveSubsystem;
8+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
9+
import net.jafama.FastMath;
10+
11+
public class TurnUntilAngleCommand extends Command {
12+
private DriveSubsystem driveSubsystem;
13+
private RobotStateSubsystem robotStateSubsystem;
14+
private double target;
15+
private double vOmega;
16+
17+
public TurnUntilAngleCommand(
18+
DriveSubsystem driveSubsystem,
19+
RobotStateSubsystem robotStateSubsystem,
20+
Rotation2d target,
21+
double vOmega) {
22+
this.driveSubsystem = driveSubsystem;
23+
this.robotStateSubsystem = robotStateSubsystem;
24+
this.target = target.getDegrees();
25+
this.vOmega = vOmega;
26+
27+
if (robotStateSubsystem.getAllianceColor() == Alliance.Red) {
28+
this.target = Units.radiansToDegrees(FastMath.normalizeZeroTwoPi(target.getRadians()));
29+
}
30+
addRequirements(driveSubsystem);
31+
}
32+
33+
@Override
34+
public void initialize() {
35+
driveSubsystem.move(0, 0, vOmega, false);
36+
}
37+
38+
@Override
39+
public void execute() {
40+
driveSubsystem.move(0, 0, vOmega, false);
41+
}
42+
43+
@Override
44+
public boolean isFinished() {
45+
if (robotStateSubsystem.getAllianceColor() == Alliance.Blue) {
46+
return driveSubsystem.getGyroRotation2d().getDegrees() <= target;
47+
} else {
48+
return driveSubsystem.getGyroRotation2d().getDegrees() >= target;
49+
}
50+
}
51+
52+
@Override
53+
public void end(boolean interrupted) {}
54+
}

‎src/main/java/frc/robot/constants/AutonConstants.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,13 @@ public final class AutonConstants {
1616
public static final double kMaxAutonVelDeadeyeDrive = 2.0;
1717
public static final double kMaxAccelDeadeyeDrive = 3.5;
1818
public static final double kXSpeed = 2.0;
19-
public static final double kMaxXOff = 1.0;
19+
public static final double kMaxXOff = 1.93; // 1.0
2020
public static final double kSwitchXLine = 6.5;
2121
public static final double kWingSwitchXLine = 1.4;
2222
public static final double kPercentLeft = 0.65;
2323
public static final double kWingPercentLeft = 0.5;
2424
public static final double kDeadeyeHuntOmegaRadps = -1.5;
25+
public static final double kDeadeyeHuntStartYawDegs = -10;
2526
public static final double kFoundNoteLoopCounts = 3;
2627

2728
public static final int kSwitchStableCounts = 100;

‎src/main/java/frc/robot/constants/DriveConstants.java‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -169,11 +169,14 @@ public static CurrentLimitsConfigs getSafeDriveLimits() {
169169

170170
public static CurrentLimitsConfigs getNormDriveLimits() {
171171
CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
172-
currentConfig.SupplyCurrentLimit = 45; // 40
173-
currentConfig.SupplyCurrentThreshold = 50; // 45
172+
currentConfig.SupplyCurrentLimit = 30; // 45
173+
currentConfig.SupplyCurrentThreshold = 35; // 50
174174
currentConfig.SupplyTimeThreshold = 0.0;
175+
176+
currentConfig.StatorCurrentLimit = 140;
177+
175178
currentConfig.SupplyCurrentLimitEnable = true;
176-
currentConfig.StatorCurrentLimitEnable = false;
179+
currentConfig.StatorCurrentLimitEnable = true;
177180
return currentConfig;
178181
}
179182

0 commit comments

Comments
 (0)