diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3704a56..0ab7359 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -105,8 +105,6 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.m_robotSwerveDrive.resetGyro(); - // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c1a95fd..b9ef435 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -77,7 +78,7 @@ public class RobotContainer { private Command emptyCommand = new InstantCommand(); private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight); - private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135)); + private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 1000), new PivotCommand(m_robotArm, 145)); private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); @@ -180,8 +181,10 @@ public class RobotContainer { new WaitCommand(0.3), toggleClaw.asProxy(), new WaitCommand(0.3), - armToHome.asProxy(), - new JoystickPlayback(m_robotSwerveDrive, "idk", 1), + new ParallelCommandGroup( + armToHome.asProxy(), + new JoystickPlayback(m_robotSwerveDrive, "idk", 1) + ), new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive) ); diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index afaa702..238e212 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -629,8 +629,8 @@ public class JoystickPlayback extends CommandBase { String[] values = line.split(","); var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * mult / 1.75; - out.leftY = Double.parseDouble(values[1]) / -1.75; + out.leftX = Double.parseDouble(values[0]) * mult / -1.68; + out.leftY = Double.parseDouble(values[1]) / -1.68; out.rightX = Double.parseDouble(values[2]); out.rightY = Double.parseDouble(values[3]);