swerve more smooth. created solenoid object

src/main/java/frc/robot/Constants.java

@@ -29,6 +29,8 @@ public static final class PortConstants {
     public static final int FRONT_RIGHT_CODER_DRIVE = 3;
     public static final int BACK_LEFT_CODER_DRIVE = 5;
     public static final int BACK_RIGHT_CODER_DRIVE = 7;
+
+    public static final int GATE = 0;
   }
 
   public static final class SwerveConstants{

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

@@ -9,8 +9,10 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.Constants.OIConstants;
+import frc.robot.commands.ResetGyro;
 import frc.robot.commands.SwerveDrive;
 import frc.robot.commands.TestRotator;
+import frc.robot.commands.ToggleGate;
 import frc.robot.subsystems.DriveTrain.DriveTrain;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
@@ -70,6 +72,17 @@ private void configureButtonBindings() {
         .whileHeld(
           new TestRotator(m_driveTrain)
         );
+
+      new JoystickButton(m_mainStick, Button.kStart.value)
+        .whenPressed(
+          new ResetGyro(m_driveTrain)
+        );
+
+      new JoystickButton(m_mainStick, Button.kX.value)
+        .whenPressed(
+          new ToggleGate(m_driveTrain)
+        );
+
     }
 
     /**

src/main/java/frc/robot/commands/ResetGyro.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DriveTrain.DriveTrain;
+
+import frc.robot.Constants.OIConstants;
+
+public class ResetGyro extends CommandBase {
+  /** Creates a new SwerveDrive. */
+  DriveTrain m_drive;
+  public ResetGyro(DriveTrain m_drive) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_drive);
+    this.m_drive = m_drive;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      m_drive.resetGyro();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/SwerveDrive.java

@@ -13,6 +13,7 @@
 
 public class SwerveDrive extends CommandBase {
   /** Creates a new SwerveDrive. */
+  private double angle;
   DriveTrain m_drive;
   private Joystick m_mainStick = new Joystick(OIConstants.mainStickPort);
   public SwerveDrive(DriveTrain m_drive) {
@@ -30,26 +31,32 @@ public void initialize() {}
   public void execute() {
     double xAngle = m_mainStick.getRawAxis(0);
     double yAngle = m_mainStick.getRawAxis(1);
-    double angle = Math.toDegrees(Math.atan((yAngle/xAngle)/2)) + 90;
-    if (xAngle < 0) {
-      angle += 180;
-    }
+
     double mag = Math.sqrt(xAngle*xAngle + yAngle*yAngle);
     if (mag > 1) {
       mag = 1;
     }
-    mag *= 0.3;
+
     double turn = m_mainStick.getRawAxis(4);
     //field oriented
-    double gyroAngle = m_drive.getGyroAngle();
-    if (gyroAngle < 0) {
-      gyroAngle += 360;
-    }
+    if (mag < 0.02) {
+      angle = angle;
+    } else {
+      angle = Math.toDegrees(Math.atan((yAngle/xAngle)/2)) + 90;
+      if (xAngle < 0) {
+        angle += 180;
+      }
+      double gyroAngle = m_drive.getGyroAngle();
+      if (gyroAngle < 0) {
+        gyroAngle += 360;
+      }
 
-    angle -= m_drive.getGyroAngle();
-    if (angle < 0) {
-      angle += 360;
+      angle -= m_drive.getGyroAngle();
+      if (angle < 0) {
+        angle += 360;
+      }
     }
+    mag *= 0.3;
 
     m_drive.m_controller.setSwerveDrive(angle, mag, turn);
 

src/main/java/frc/robot/commands/ToggleGate.java

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DriveTrain.DriveTrain;
+
+public class ToggleGate extends CommandBase {
+  /** Creates a new ToggleCompressor. */
+  DriveTrain m_drive;
+  public ToggleGate(DriveTrain m_drive) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_drive);
+    this.m_drive = m_drive;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if (m_drive.getGateStatus()) {
+      m_drive.setGate(false);
+    } else {
+      m_drive.setGate(true);
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/subsystems/DriveTrain/DriveTrain.java

@@ -19,9 +19,13 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.SerialPort.Port;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.OIConstants;
 import frc.robot.Constants.PortConstants;
@@ -55,6 +59,8 @@ public class DriveTrain extends SubsystemBase{
   private Translation2d m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation;
   private SwerveDriveKinematics m_kinematics;
 
+  private Solenoid m_gate;
+
   double wkP, wkI, wkD;
 //
 
@@ -135,16 +141,28 @@ public DriveTrain() {
     );
 
     m_gyro.zeroYaw();
+    //m_gate = new Solenoid(PneumaticsModuleType.CTREPCM, PortConstants.GATE);
   }
 
   public double getGyroAngle() {
     return m_gyro.getYaw();
   }
 
+  public void resetGyro() {
+    m_gyro.zeroYaw();
+  }
+
   public void turnBackRight(double speed) {
     m_backRightRotationMotor.set(TalonFXControlMode.PercentOutput, speed);
   }
 
+  public void setGate(boolean b) {
+    m_gate.set(b);
+  }
+
+  public boolean getGateStatus() {
+    return m_gate.get();
+  }
 
   @Override
   public void periodic() {
@@ -170,5 +188,6 @@ public void periodic() {
     SmartDashboard.putNumber("FRCoder", m_frontRightCoder.getAbsolutePosition());
     SmartDashboard.putNumber("BLCoder", m_backLeftCoder.getAbsolutePosition());
     SmartDashboard.putNumber("BRCoder", m_backRightCoder.getAbsolutePosition());
+    SmartDashboard.putBoolean("Solenoid status", m_gate.get());
   }
 }

src/main/java/frc/robot/subsystems/DriveTrain/SwerveDriveController.java

@@ -30,6 +30,7 @@ public void setSwerveDrive(double driveDirection, double driveSpeed, double turn
             //either only drives if turnspeed is 0
             //or drives and turns at the same time if turnspeed is not 0
             this.driveTurn(driveDirection, driveSpeed, turnSpeed);
+
         }
     }
 
@@ -57,9 +58,9 @@ public void turn(double speed) {
     public void driveTurn(double driveDirection, double driveSpeed, double turnSpeed) {
         //turn Speed is value fron -1.0 to 1.0 with -1 being max left and 1 being max right
         double turnAngle = turnSpeed * 45.0;
-        driveSpeed *= Math.abs(turnSpeed)*0.45 + 1;
+        //while turning, can turn a maximum of 40 degrees
+        driveSpeed *= (Math.log(Math.abs(turnSpeed)+1)/Math.log(3))*0.45 + 1;
         SmartDashboard.putNumber("turn Angle", turnAngle);
-
         //determine if wheel is in front or in back
         //for example: if the direction was to the right and the robot was facing forward,
         //the frontRight and backRight wheel would be front and the frontLeft and backLeft wheels would be in back