diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index fb65005..e0de605 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -51,10 +51,10 @@ public class AprilTag3 extends Command { double b = (limelight3.getTA()*100) / (Math.cos(90-limelight3.getTx())); if(limelight3.getV() == true){ drivetrain.setControl(drive. - withRotationalRate(a/10). + withRotationalRate(a/20). withVelocityX(0). - withVelocityY( b/30)); - System.out.println(b/10); + withVelocityY( b/50)); + System.out.println(b/50); } else{ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 615bf6d..314d3f7 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType;