Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 08:48:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Make boiler plate code - Needs to be programmed with robot"

Browse Source 
This reverts commit 0f7861ff02.
This commit is contained in:
Astatin3
2024-02-15 15:35:53 -07:00
parent 1d04fc34de
commit 2825af3cde
11 changed files with 241 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/Autos/AutoAlign.java
-62
View File
@@ -1,62 +0,0 @@
package frc4388.robot.commands.Autos;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.Constants.AutoAlignConstants;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
public class AutoAlign extends Command {
private SwerveDrive swerve;
private Limelight limelight;
private Pose2d pose;
private boolean isRed = DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
public AutoAlign(SwerveDrive swerve, Limelight limelight) {
this.swerve = swerve;
}
private Translation2d calcMoveStick(){
//TODO - DO THE MATH!
pose = limelight.getPose();
double curX = pose.getX();
double curY = pose.getY();
double curYaw = pose.getRotation().getDegrees();
// This is not math
double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed;
double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed;
return new Translation2d(stickX, stickY);
}
private Translation2d calcRotStick(){
//TODO - DO THE MATH!
pose = limelight.getPose();
double curX = pose.getX();
double curY = pose.getY();
double curYaw = pose.getRotation().getDegrees();
// This is not math
double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed;
double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed;
return new Translation2d(stickX, stickY);
}
public void execute() {
Translation2d moveStick = calcMoveStick();
Translation2d rotStick = calcRotStick();
// This would greatly benifit from having feild Relative implemented.
swerve.driveWithInput(moveStick, rotStick, false);
}
}
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
+1 -7
View File
@@ -66,13 +66,7 @@ public class PlaybackChooser {
public void nextChooser() {
SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
String[] dirs = m_dir.list();
if(dirs == null){ // Fix funny error
return;
}
for (String auto : dirs) {
for (String auto : m_dir.list()) {
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
}
for (var cmd_name : m_commandPool.keySet()) {