diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5924a25..3b8b887 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,7 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; - +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -34,6 +34,7 @@ import frc.robot.commands.bras.PivoteBrasMilieux; public class RobotContainer { + CameraServer.startAutomaticCapture(); CommandXboxController manette1 = new CommandXboxController(0); CommandXboxController manette2 = new CommandXboxController(1); // subsystems diff --git a/src/main/java/frc/robot/commands/Gyro.java b/src/main/java/frc/robot/commands/Gyro.java index a20e5aa..e8742ba 100644 --- a/src/main/java/frc/robot/commands/Gyro.java +++ b/src/main/java/frc/robot/commands/Gyro.java @@ -28,13 +28,13 @@ public class Gyro extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(basePilotable.getpitch()<10) + if(basePilotable.getpitch()>4) { - basePilotable.drive(0.4, 0); + basePilotable.drive(0.3*basePilotable.getpitch()/15, 0); } - else if(basePilotable.getpitch()>-10) + else if(basePilotable.getpitch()<-4) { - basePilotable.drive(-0.4, 0); + basePilotable.drive(0.3*basePilotable.getpitch()/15, 0); } else {