Skip to content

Commit 4c1185a

Browse files
Swordman51Swordman51
committed
more changes
1 parent 1681024 commit 4c1185a

File tree

6 files changed

+41
-31
lines changed

6 files changed

+41
-31
lines changed

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

+1
Original file line numberDiff line numberDiff line change
@@ -529,6 +529,7 @@ public Command shootNote() {
529529
.alongWith(
530530
new SequentialCommandGroup(
531531
indexer.manualCommand(IndexerConstants.INDEXER_OUT_VOLTAGE / 2).withTimeout(0.1), // run intake back for 0.1 seconds
532+
new WaitUntilCommand(0.2),
532533
indexer.manualCommand(IndexerConstants.INDEXER_IN_VOLTAGE) // run intake in to shoot
533534
))
534535
.withTimeout(1.5);//.alongWith(new InstantCommand(() -> {NoteVisualizer.shoot().schedule();})); // run the visualizer

‎src/main/java/frc/robot/commands/DriveCommands.java

+28-23
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,16 @@
1313

1414
package frc.robot.commands;
1515

16+
import static frc.robot.subsystems.drive.DriveConstants.kSlowModeConstant;
17+
import static frc.robot.subsystems.drive.DriveConstants.kTurnSpeakerD;
18+
import static frc.robot.subsystems.drive.DriveConstants.kTurnSpeakerI;
19+
import static frc.robot.subsystems.drive.DriveConstants.kTurnSpeakerP;
20+
21+
import java.util.function.DoubleSupplier;
22+
import java.util.function.Supplier;
23+
24+
import org.littletonrobotics.junction.Logger;
25+
1626
import edu.wpi.first.math.MathUtil;
1727
import edu.wpi.first.math.controller.PIDController;
1828
import edu.wpi.first.math.geometry.Pose2d;
@@ -26,24 +36,9 @@
2636
import edu.wpi.first.wpilibj2.command.Command;
2737
import edu.wpi.first.wpilibj2.command.Commands;
2838
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
29-
import edu.wpi.first.wpilibj2.command.WaitCommand;
30-
import edu.wpi.first.wpilibj2.command.WaitCommand;
31-
import frc.robot.subsystems.indexer.Indexer;
32-
import frc.robot.subsystems.indexer.IndexerConstants;
33-
import frc.robot.subsystems.pivotArm.PivotArm;
34-
import frc.robot.subsystems.pivotArm.PivotArmConstants;
3539
import frc.robot.FieldConstants;
3640
import frc.robot.subsystems.drive.Drive;
37-
import frc.robot.subsystems.pivotArm.PivotArm;
38-
import frc.robot.subsystems.pivotArm.PivotArmConstants;
39-
import frc.robot.subsystems.shooter.*;
40-
import java.util.function.DoubleSupplier;
41-
import java.util.function.Supplier;
42-
43-
import org.littletonrobotics.junction.Logger;
44-
45-
import static frc.robot.subsystems.drive.DriveConstants.*;
46-
import static frc.robot.util.drive.DriveControls.DRIVE_AMP;
41+
import frc.robot.subsystems.drive.DriveConstants;
4742

4843
public class DriveCommands {
4944
private static final double DEADBAND = 0.1;
@@ -92,8 +87,9 @@ public static Command joystickDrive(
9287
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
9388
omega * drive.getMaxAngularSpeedRadPerSec(),
9489
isFlipped
95-
? drive.getRotation()/* .plus(new Rotation2d(Math.PI)) */
96-
: drive.getRotation()));
90+
? drive.getRotation().plus(new Rotation2d(Math.PI))
91+
: drive.getRotation()
92+
));
9793
},
9894
drive);
9995
}
@@ -132,7 +128,7 @@ public static Command joystickDriveRobotRelative(
132128
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
133129
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
134130
omega * drive.getMaxAngularSpeedRadPerSec(),
135-
drive.getRotation()));
131+
new Rotation2d()));
136132
},
137133
drive);
138134
}
@@ -191,7 +187,10 @@ public static Command joystickSpeakerPoint(
191187
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
192188
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
193189
omega * drive.getMaxAngularSpeedRadPerSec(),
194-
drive.getRotation()));
190+
getIsFlipped()
191+
? drive.getRotation().plus(new Rotation2d(Math.PI))
192+
: drive.getRotation()
193+
));
195194
},
196195
drive);
197196
}
@@ -240,7 +239,10 @@ public static Command joystickPasserPoint(
240239
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
241240
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
242241
omega * drive.getMaxAngularSpeedRadPerSec(),
243-
drive.getRotation()));
242+
getIsFlipped()
243+
? drive.getRotation().plus(new Rotation2d(Math.PI))
244+
: drive.getRotation()
245+
));
244246
},
245247
drive);
246248
}
@@ -280,7 +282,10 @@ public static Command joystickAnglePoint(
280282
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
281283
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
282284
omega * drive.getMaxAngularSpeedRadPerSec(),
283-
drive.getRotation()));
285+
getIsFlipped()
286+
? drive.getRotation().plus(new Rotation2d(Math.PI))
287+
: drive.getRotation()
288+
));
284289
},
285290
drive);
286291
}
@@ -334,7 +339,7 @@ public static boolean pointedAtSpeaker(Drive drive) {
334339
Rotation2d targetDirection = new Rotation2d(targetTransform.getX(), targetTransform.getY()).plus(new Rotation2d(getIsFlipped() ? Math.PI : 0));;
335340

336341
// Convert to robot relative speeds and send command
337-
if (Math.abs(drive.getRotation().getDegrees() - targetDirection.getDegrees()) < 1) {
342+
if (Math.abs(drive.getRotation().getDegrees() - targetDirection.getDegrees()) < DriveConstants.angleThreshold) {
338343
return true;
339344
} else {
340345
return false;

‎src/main/java/frc/robot/subsystems/drive/DriveConstants.java

+2
Original file line numberDiff line numberDiff line change
@@ -67,5 +67,7 @@ public final class DriveConstants {
6767
public static final double kTurnSpeakerD = 0;
6868
public static final double kTurnSpeakerTolerance = 0.05;
6969
public static final double kTurnSpeakerRateTolerance = 0.02;
70+
71+
public static final double angleThreshold = 3;
7072

7173
}

‎src/main/java/frc/robot/subsystems/indexer/Indexer.java

+3-2
Original file line numberDiff line numberDiff line change
@@ -103,7 +103,8 @@ public boolean isIntaked() {
103103

104104
// Sets motor speed based on where the note is in the intake
105105
public void runIntakeLoop() {
106-
switch(noteState) {
106+
setVoltage(currentVoltage);
107+
/* switch(noteState) {
107108
case NOT_ENOUGH:
108109
setVoltage(currentVoltage);
109110
break;
@@ -113,7 +114,7 @@ public void runIntakeLoop() {
113114
case MIDDLE: case OVERSHOOT:
114115
setVoltage(-currentVoltage);
115116
break;
116-
}
117+
} */
117118
}
118119

119120
// Checks if note has been in the intake for 0.5 seconds

‎src/main/java/frc/robot/subsystems/pivotArm/PivotArmConstants.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public class PivotArmConstants {
1515
public static final double PIVOT_ARM_PID_TOLERANCE = Units.degreesToRadians(1);
1616
public static final double PIVOT_ARM_PID_VELOCITY_TOLERANCE = 0.5;
1717

18-
public static final double PIVOT_ARM_OFFSET = 1.1;// 1.14;
18+
public static final double PIVOT_ARM_OFFSET = 1.5;// 1.14;
1919

2020
public static final double PIVOT_MAX_PID_TIME = 3;
2121

‎src/main/java/frc/robot/subsystems/shooter/Shooter.java

+6-5
Original file line numberDiff line numberDiff line change
@@ -168,17 +168,18 @@ public Command runVoltage(double volts) {
168168
public void setVoltage(DoubleSupplier leftVoltage, DoubleSupplier rightVoltage){
169169
leftMotorVoltage = leftVoltage.getAsDouble() * 10;
170170
rightMotorVoltage = rightVoltage.getAsDouble() * 10;
171-
shooterIO.setVoltage(leftMotorVoltage, rightMotorVoltage);
172-
}
173171

174-
public void setVoltage(double volts){
175172
if(SmartDashboard.getBoolean("Turbo Mode", false)){
176-
shooterIO.setVoltage(0, 0);
173+
shooterIO.setVoltage(0, 0);
177174
} else {
178-
shooterIO.setVoltage(volts, volts);
175+
shooterIO.setVoltage(leftMotorVoltage, rightMotorVoltage);
179176
}
180177
}
181178

179+
public void setVoltage(double volts){
180+
setVoltage(() -> volts, () -> volts);
181+
}
182+
182183

183184
public void setRPM(double leftRPM, double rightRPM) {
184185
leftSetpointRPM = leftRPM; //RPM setpoint is being set here

0 commit comments

Comments
 (0)