Swerve oscillations

We are programming our swerve drive and we are having problems with the wheel oscillating in a seemingly random manner. We have tried tuning out PID however that doesn’t seem to work. Here is our code. Does anyone know what is wrong?

/----------------------------------------------------------------------------/
/* Copyright © 2018 FIRST. All Rights Reserved. /
/
Open Source Software - may be modified and shared by FRC teams. The code /
/
must be accompanied by the FIRST BSD license file in the root directory of /
/
the project. /
/
----------------------------------------------------------------------------*/

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.ctre.phoenix.motorcontrol.;
import com.ctre.phoenix.motorcontrol.can.
;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import frc.robot.OI;
import edu.wpi.first.wpilibj.Joystick;

/**

  • Add your docs here.
    */

public class DriveSubsystem extends Subsystem {
Joystick _joy = new Joystick(0);
double R,A,B,C,D,ws1,ws2,ws3,ws4,wa1,wa2,wa3,wa4,max,currentAngle,rotationAmmount,FWD,STR,RCW,currentAngle2,currentAngle3,currentAngle4,rotationAmmount2,rotationAmmount3,rotationAmmount4; //defining variables for formulas
boolean ran = false;
//defining motor controlers and encoders
WPI_TalonSRX SRXsteer = new WPI_TalonSRX(OI.SRXtoprightsteer);
WPI_TalonSRX SRXsteer2 = new WPI_TalonSRX(OI.SRXtopleftsteer);
WPI_TalonSRX SRXsteer3 = new WPI_TalonSRX(OI.SRXbottomleftsteer);
WPI_TalonSRX SRXsteer4 = new WPI_TalonSRX(OI.SRXbottomrightsteer);
CANSparkMax MAXdrive = new CANSparkMax(OI.MAXtoprightdrive, MotorType.kBrushless);
public CANPIDController drive = new CANPIDController(MAXdrive);
public CANEncoder Encoder = new CANEncoder(MAXdrive);
//defining motor controlers and encoders
public DriveSubsystem() {

}

@Override
public void initDefaultCommand() {
MAXdrive.setMotorType(MotorType.kBrushless); // defining motor type
}
public void init() {

	 // PID config
	 drive.setP(1);
	 drive.setI(0);
	 drive.setD(0);
   
   int kTimeoutMs = 10;
   int kPIDLoopIdx = 0;
   
    int absolutePosition = SRXsteer.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;
   
	SRXsteer.setSelectedSensorPosition(absolutePosition,kPIDLoopIdx, kTimeoutMs);
	SRXsteer.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,kPIDLoopIdx,kTimeoutMs);
	SRXsteer.setSensorPhase(true);

	SRXsteer.configNominalOutputForward(0,kTimeoutMs);
	SRXsteer.configNominalOutputReverse(0,kTimeoutMs);
	SRXsteer.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer.configPeakOutputReverse(-1,kTimeoutMs);
	SRXsteer.configAllowableClosedloopError(0,kPIDLoopIdx,kTimeoutMs);
	   
	SRXsteer.config_kF(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer.config_kP(kPIDLoopIdx, .5,kTimeoutMs);
	SRXsteer.config_kI(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer.config_kD(kPIDLoopIdx, 1,kTimeoutMs);
	SRXsteer.setSelectedSensorPosition(0, 0, 0);
	  
	int absolutePosition2 = SRXsteer2.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;
	  
	SRXsteer2.setSelectedSensorPosition(absolutePosition2,kPIDLoopIdx, kTimeoutMs);
	SRXsteer2.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,kPIDLoopIdx,kTimeoutMs);
	SRXsteer2.setSensorPhase(true);
		 
	SRXsteer2.configNominalOutputForward(0,kTimeoutMs);
	SRXsteer2.configNominalOutputReverse(0,kTimeoutMs);
	SRXsteer2.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer2.configPeakOutputReverse(-1,kTimeoutMs);
	SRXsteer2.configAllowableClosedloopError(0,kPIDLoopIdx,kTimeoutMs);
		  
	SRXsteer2.config_kF(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer2.config_kP(kPIDLoopIdx, .5,kTimeoutMs);
	SRXsteer2.config_kI(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer2.config_kD(kPIDLoopIdx, 1,kTimeoutMs);
	SRXsteer2.setSelectedSensorPosition(0, 0, 0); 

	int absolutePosition3 = SRXsteer3.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;
	  
	SRXsteer3.setSelectedSensorPosition(absolutePosition3,kPIDLoopIdx, kTimeoutMs);
	SRXsteer3.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,kPIDLoopIdx,kTimeoutMs);
	SRXsteer3.setSensorPhase(true);
		 
	SRXsteer3.configNominalOutputForward(0,kTimeoutMs);
	SRXsteer3.configNominalOutputReverse(0,kTimeoutMs);
	SRXsteer3.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer3.configPeakOutputReverse(-1,kTimeoutMs);
	SRXsteer3.configAllowableClosedloopError(0,kPIDLoopIdx,kTimeoutMs);
		  
	SRXsteer3.config_kF(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer3.config_kP(kPIDLoopIdx, .5,kTimeoutMs);
	SRXsteer3.config_kI(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer3.config_kD(kPIDLoopIdx, 1,kTimeoutMs);
	SRXsteer3.setSelectedSensorPosition(0, 0, 0);  
	
	int absolutePosition4 = SRXsteer4.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;
	  
	SRXsteer4.setSelectedSensorPosition(absolutePosition4,kPIDLoopIdx, kTimeoutMs);
	SRXsteer4.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,kPIDLoopIdx,kTimeoutMs);
	SRXsteer4.setSensorPhase(true);

	SRXsteer4.configNominalOutputForward(0,kTimeoutMs);
	SRXsteer4.configNominalOutputReverse(0,kTimeoutMs);
	SRXsteer4.configPeakOutputForward(1, kTimeoutMs);
	SRXsteer4.configPeakOutputReverse(-1,kTimeoutMs);
		  
	SRXsteer4.configAllowableClosedloopError(0,kPIDLoopIdx,kTimeoutMs);
		  
	SRXsteer4.config_kF(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer4.config_kP(kPIDLoopIdx, .5,kTimeoutMs);
	SRXsteer4.config_kI(kPIDLoopIdx, 0.0,kTimeoutMs);
	SRXsteer4.config_kD(kPIDLoopIdx, 1,kTimeoutMs);
	SRXsteer4.setSelectedSensorPosition(0, 0, 0); 
	
	SmartDashboard.putNumber("CHECK", 1);
	
   // PID config

}
public void Swerve() {

currentAngle = SRXsteer.getSelectedSensorPosition(0)/25.930555555555; // setting the current angle of the wheel 11.4666 = tick per rotation/360
currentAngle2 = SRXsteer2.getSelectedSensorPosition(0)/25.930555555555;
currentAngle3 = SRXsteer3.getSelectedSensorPosition(0)/25.930555555555;	
currentAngle4 = SRXsteer4.getSelectedSensorPosition(0)/25.930555555555;
//swerve formulas
	FWD = -_joy.getRawAxis(1);
	STR = -_joy.getRawAxis(0);
	RCW = -_joy.getRawAxis(2);
	R = Math.sqrt( (OI.L*OI.L)+(OI.W*OI.W) );
	A = STR - RCW*(OI.L/R);
	B = STR + RCW*(OI.L/R);
	C = FWD - RCW*(OI.W/R);
	D = FWD + RCW*(OI.W/R);
	ws1 = Math.sqrt( (B*B)+(C*C) );
	ws2 = Math.sqrt( (B*B)+(D*D) );
	ws3 = Math.sqrt( (A*A)+(D*D) );	
	ws4 = Math.sqrt( (A*A)+(C*C) );
	wa1 = Math.atan2(B,C)*180/Math.PI;
	wa2 = Math.atan2(B,D)*180/Math.PI;
	wa3 = Math.atan2(A,D)*180/Math.PI;
	wa4 = Math.atan2(A,C)*180/Math.PI;
    max = ws1;
    if(ws2 > max) {
    	max = ws2;
    }    
    if(ws3 > max) {
    	max = ws3;
    }
    if(ws4 > max) {
    	max = ws4;
    }
    if (max > 1) {
    	ws1/=max;
    	ws2/=max;
    	ws3/=max;
    	ws4/=max;
	}
	//swerve formulas

	

	//smartdashboard puts

	//smartdashboard puts
	SmartDashboard.putNumber("Encoder4 Position", SRXsteer4.getSelectedSensorPosition(0));
	SmartDashboard.putNumber("Encoder3 Position", SRXsteer3.getSelectedSensorPosition(0));

	SmartDashboard.putNumber("angle4", currentAngle4);
	SmartDashboard.putNumber("output4", rotationAmmount4);
	SmartDashboard.putNumber("trtwt4", wa4-currentAngle4);	
	SmartDashboard.putNumber("angle3", currentAngle);
	SmartDashboard.putNumber("output3", rotationAmmount3);
	SmartDashboard.putNumber("trtwt3", wa4-currentAngle3);	

	SmartDashboard.putNumber("WheelAngle", wa1);
	SmartDashboard.putNumber("WheenSpeed", ws1);
	SmartDashboard.putNumber("WheelAngle2", wa2);
	SmartDashboard.putNumber("WheenSpeed2", ws2);
	SmartDashboard.putNumber("WheelAngle3", wa3);
	SmartDashboard.putNumber("WheenSpeed3", ws3);
	SmartDashboard.putNumber("WheelAngle4", wa4);
	SmartDashboard.putNumber("WheenSpeed4", ws4);
	SmartDashboard.putNumber("RotaitonAmount", rotationAmmount);
//	System.out.println(rotationAmmount);

	rotationAmmount = Math.IEEEremainder(wa1-currentAngle,360); // calculating ammount to move wheel
	rotationAmmount2 = Math.IEEEremainder(wa2-currentAngle2,360); // calculating ammount to move wheel
	rotationAmmount3 = Math.IEEEremainder(wa3-currentAngle3,360); // calculating ammount to move wheel
	rotationAmmount4 = Math.IEEEremainder(wa4-currentAngle4,360); // calculating ammount to move wheel

	MAXdrive.set(ws1); // drining drive wheel off the formulas
	SRXsteer.set(ControlMode.Position,(currentAngle+rotationAmmount)*25.930555555555); // moving the wheel to the required position
	SRXsteer2.set(ControlMode.Position,(currentAngle2+rotationAmmount2)*25.930555555555); // moving the wheel to the required position
	SRXsteer3.set(ControlMode.Position,(currentAngle3+rotationAmmount3)*25.930555555555); // moving the wheel to the required position
	SRXsteer4.set(ControlMode.Position,(currentAngle4+rotationAmmount4)*25.930555555555); // moving the wheel to the required position
}

public void driveForward(double distance) {
currentAngle = SRXsteer.getSelectedSensorPosition(0)/25.930555555555; // setting the current angle of the wheel 11.4666 = tick per rotation/360
rotationAmmount = Math.IEEEremainder(0-currentAngle,360); // calculating ammount to move wheel
SRXsteer.set(ControlMode.Position,(currentAngle+rotationAmmount)*25.930555555555); // moving the wheel to the required position
drive.setReference(distance, ControlType.kPosition); //moving drive to target position
}
}

Just an update here a video of the swerve https://www.chiefdelphi.com/uploads/default/original/3X/4/e/4e509622e4ab3f0408d3d7724f3043de1b0a9f78.mp4

Please edit your post and properly format the code block, or ideally link to a github(or similar) repository for us to look at.

Having said that, oscillations are usually caused by poorly tuned PID constants, which isn’t something we can easily troubleshoot remotely, since it’s 100% dependent on the hardware