Mr. Purring with wheel independent PID control

Mr. Purring with wheel independent PID control

// 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;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.UsbCamera;
import edu.wpi.first.math.controller.PIDController;


public class Robot extends TimedRobot {

//Declaring The port that the xbox controller will use in smart dashboard
    private XboxController cplusplus = new XboxController(0);
  
//This is setting up the camera to read from usb port zero
UsbCamera Cameranumber1 = CameraServer.startAutomaticCapture(0); 

//Motor names and their respective pwm ports
    private final MotorController front_leftMotor = new PWMSparkMax(2);
    private final MotorController front_rightMotor = new PWMSparkMax(0);
    private final MotorController rear_leftMotor = new PWMSparkMax(3);
    private final MotorController rear_rightMotor = new PWMSparkMax(1);
  
//Encoder Names
    Encoder encoder1, encoder2, encoder3, encoder4;

//PID controllers
    PIDController pid_front_left, pid_rear_left, pid_front_right, pid_rear_right;
    double kp = 0; 
    double ki = 0;
    double kd = 0;
   
//Values for encoders
    private static final double cpr = 360; //this is representative of the resolution of the encoders
    private static final double whd = 8;  //this variable is representative of the 8 inch wheel diameter

@Override
public void robotInit() {

/* We need to invert one side of the drivetrain so that positive voltages
result in both sides moving forward*/ 
    front_rightMotor.setInverted(true);
    rear_rightMotor.setInverted(true);


//Declaring the ports that the encoders are plugged into
    encoder1 = new Encoder(0,1);
    encoder2 = new Encoder(2,3);
    encoder3 = new Encoder(4,5);
    encoder4 = new Encoder(6,7);

//Setting the distance per pulse on encoders
    encoder1.setDistancePerPulse(Math.PI*whd/cpr);
    encoder2.setDistancePerPulse(Math.PI*whd/cpr);
    encoder3.setDistancePerPulse(Math.PI*whd/cpr);
    encoder4.setDistancePerPulse(Math.PI*whd/cpr);

//Declaring the names of the PID controllers     
    pid_front_left = new PIDController(kp, ki, kd);
    pid_rear_left = new PIDController(kp, ki, kd);
    pid_front_right = new PIDController(kp, ki, kd);
    pid_rear_right = new PIDController(kp, ki, kd);
}
    @Override

public void teleopPeriodic() {

//PID controls being used to set motor speed
   front_leftMotor.set((pid_front_left.calculate(encoder1.getRate(), cplusplus.getRawAxis(1))));
   front_rightMotor.set((pid_rear_left.calculate(encoder2.getRate(), cplusplus.getRawAxis(1))));
   rear_leftMotor.set((pid_front_right.calculate(encoder3.getRate(), cplusplus.getRawAxis(5))));
   rear_rightMotor.set((pid_rear_right.calculate(encoder4.getRate(), cplusplus.getRawAxis(5))));


//Displaying the encoder values to Smart dashboard
   SmartDashboard.putNumber("Encoder1", encoder1.getRate());
   SmartDashboard.putNumber("Encoder2", encoder2.getRate());
   SmartDashboard.putNumber("Encoder3", encoder3.getRate());
   SmartDashboard.putNumber("Encoder4", encoder4.getRate());
  
  }
}