DriveTrain.java

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

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.FollowerType;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
import com.ctre.phoenix.motorcontrol.TalonFXInvertType;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Robot;
import frc.robot.Constants.DriveTrainConstants;
import frc.robot.sim.PhysicsSim;

// https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20Talon%20FX%20(Falcon%20500)/DriveStraight_AuxIntegratedSensor/src/main/java/frc/robot/Robot.java
// https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20Talon%20FX%20(Falcon%20500)/DifferentialDrive/src/main/java/frc/robot/Robot.java
public class DriveTrain extends SubsystemBase {
	/** Creates a new DriveTrain. */
	private WPI_TalonFX leftMaster = new WPI_TalonFX(DriveTrainConstants.MOTOR_LEFT_MASTER_ID),
			leftFollower = new WPI_TalonFX(DriveTrainConstants.MOTOR_LEFT_FOLLOWER_ID),
			rightMaster = new WPI_TalonFX(DriveTrainConstants.MOTOR_RIGHT_MASTER_ID),
			rightFollower = new WPI_TalonFX(DriveTrainConstants.MOTOR_RIGHT_FOLLOWER_ID);

	private final DifferentialDrive diffDrive = new DifferentialDrive(leftMaster, rightMaster);

	private TalonFXConfiguration leftConfig = new TalonFXConfiguration(),
			rightConfig = new TalonFXConfiguration();

	@Override
	public void simulationPeriodic() {
		PhysicsSim.getInstance().run();
	}

	public DriveTrain() {
		if (!Robot.isReal()) {
			PhysicsSim.getInstance().addTalonFX(leftMaster, 0.75, 4000);
			PhysicsSim.getInstance().addTalonFX(rightMaster, 0.75, 4000);
			PhysicsSim.getInstance().addTalonFX(leftFollower, 0.75, 4000);
			PhysicsSim.getInstance().addTalonFX(rightFollower, 0.75, 4000);
		}

		/* factory default values */
		rightMaster.configFactoryDefault();
		rightFollower.configFactoryDefault();
		leftMaster.configFactoryDefault();
		leftFollower.configFactoryDefault();

		/* set up followers */
		rightFollower.follow(rightMaster);
		leftFollower.follow(leftMaster);

		/* [3] flip values so robot moves forward when stick-forward/LEDs-green */
		rightMaster.setInverted(DriveTrainConstants.RIGHT_MOTORS_INVERT);
		leftMaster.setInverted(DriveTrainConstants.LEFT_MOTORS_INVERT);

		/* set up follower inversions */
		rightFollower.setInverted(InvertType.FollowMaster);
		leftFollower.setInverted(InvertType.FollowMaster);

		/* Set Neutral Mode */
		leftMaster.setNeutralMode(NeutralMode.Brake);
		rightMaster.setNeutralMode(NeutralMode.Brake);

		// rightMaster.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
		// leftMaster.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);

		/** Closed loop configuration */

		/* Configure the left Talon's selected sensor as integrated sensor */
		/*
		 * Currently, in order to use a product-specific FeedbackDevice in configAll
		 * objects,
		 * you have to call toFeedbackType. This is a workaround until a
		 * product-specific
		 * FeedbackDevice is implemented for configSensorTerm
		 */
		leftConfig.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); // Local
																													// Feedback
																													// Source

		/*
		 * Configure the Remote (Left) Talon's selected sensor as a remote sensor for
		 * the right Talon
		 */
		rightConfig.remoteFilter1.remoteSensorDeviceID = leftMaster.getDeviceID(); // Device ID of Remote Source
		rightConfig.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; // Remote Source Type

		/*
		 * Setup difference signal to be used for turn when performing Drive Straight
		 * with encoders
		 */
		setRobotTurnConfigs(DriveTrainConstants.RIGHT_MOTORS_INVERT, rightConfig);

		/* Config the neutral deadband. */
		leftConfig.neutralDeadband = DriveTrainConstants.NEUTRAL_DEADBAND;
		rightConfig.neutralDeadband = DriveTrainConstants.NEUTRAL_DEADBAND;

		/*
		 * max out the peak output (for all modes). However you can
		 * limit the output of a given PID object with configClosedLoopPeakOutput().
		 */
		leftConfig.peakOutputForward = +1.0;
		leftConfig.peakOutputReverse = -1.0;
		rightConfig.peakOutputForward = +1.0;
		rightConfig.peakOutputReverse = -1.0;

		/* FPID Gains for turn servo */
		/* FPID for Distance */
		rightConfig.slot1.kF = DriveTrainConstants.GAINS_TURNING.F;
		rightConfig.slot1.kP = DriveTrainConstants.GAINS_TURNING.P;
		rightConfig.slot1.kI = DriveTrainConstants.GAINS_TURNING.I;
		rightConfig.slot1.kD = DriveTrainConstants.GAINS_TURNING.D;
		rightConfig.slot1.integralZone = DriveTrainConstants.GAINS_TURNING.INTEGRAL_ZONE;
		rightConfig.slot1.closedLoopPeakOutput = DriveTrainConstants.GAINS_TURNING.PEAK_OUTPUT;

		/*
		 * 1ms per loop. PID loop can be slowed down if need be.
		 * For example,
		 * - if sensor updates are too slow
		 * - sensor deltas are very small per update, so derivative error never gets
		 * large enough to be useful.
		 * - sensor movement is very slow causing the derivative error to be near zero.
		 */
		int closedLoopTimeMs = 1;
		rightConfig.slot0.closedLoopPeriod = closedLoopTimeMs;
		rightConfig.slot1.closedLoopPeriod = closedLoopTimeMs;
		rightConfig.slot2.closedLoopPeriod = closedLoopTimeMs;
		rightConfig.slot3.closedLoopPeriod = closedLoopTimeMs;

		/* APPLY the config settings */
		leftMaster.configAllSettings(leftConfig);
		rightMaster.configAllSettings(rightConfig);

		/* Set status frame periods */
		rightMaster.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveTrainConstants.TIMEOUT_MS);
		rightMaster.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveTrainConstants.TIMEOUT_MS);
		leftMaster.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveTrainConstants.TIMEOUT_MS); // Used
																											// remotely
																											// by right
																											// Talon,
																											// speed up

		/* Initialize */
		zeroSensors();
	}

	public void arcadeDrive(double forward, double turn) {
		diffDrive.arcadeDrive(forward, turn);
	}

	public double getRightMotorsDistance() {
		return rightMaster.getSelectedSensorPosition();
	}

	public double getLeftMotorsDistance() {
		return leftMaster.getSelectedSensorPosition();
	}

	public void printRightMotorsDistance() {
		System.out.println("Right Motor 1 distance: " + rightMaster.getSelectedSensorPosition());
		System.out.println("Right Motor 2 distance: " + rightFollower.getSelectedSensorPosition());
	}

	public void printLeftMotorsDistance() {
		System.out.println("Left Motor 1 distance: " + leftMaster.getSelectedSensorPosition());
		System.out.println("Left Motor 2 distance: " + leftFollower.getSelectedSensorPosition());
	}

	public void setMaxOutput(double maxOutput) {
		diffDrive.setMaxOutput(maxOutput);
	}

	@Override
	public void periodic() {
		// This method will be called once per scheduler run
		printLeftMotorsDistance();
		printRightMotorsDistance();
	}

	public void selectTurningPIDSlot() {
		rightMaster.selectProfileSlot(DriveTrainConstants.SLOT_TURNING, DriveTrainConstants.PID_TURN);
	}

	public void driveStraight(double percentOutput) {
		double targetAngle = rightMaster.getSelectedSensorPosition(1);
		/*
		 * Configured for percentOutput with Auxiliary PID on Integrated Sensors'
		 * Difference
		 */
		rightMaster.set(ControlMode.PercentOutput, percentOutput, DemandType.AuxPID, targetAngle);
		leftMaster.follow(rightMaster, FollowerType.AuxOutput1);
	}

	/* Zero all sensors used */
	void zeroSensors() {
		leftMaster.getSensorCollection().setIntegratedSensorPosition(0, DriveTrainConstants.TIMEOUT_MS);
		rightMaster.getSensorCollection().setIntegratedSensorPosition(0, DriveTrainConstants.TIMEOUT_MS);

		leftFollower.getSensorCollection().setIntegratedSensorPosition(0, DriveTrainConstants.TIMEOUT_MS);
		rightFollower.getSensorCollection().setIntegratedSensorPosition(0, DriveTrainConstants.TIMEOUT_MS);
		System.out.println("[Integrated Sensors] All sensors are zeroed.\n");
	}

	/**
	 * Determines if SensorSum or SensorDiff should be used
	 * for combining left/right sensors into Robot Distance.
	 * 
	 * Assumes Aux Position is set as Remote Sensor 0.
	 * 
	 * configAllSettings must still be called on the master config
	 * after this function modifies the config values.
	 * 
	 * @param masterInvertType Invert of the Master Talon
	 * @param masterConfig     Configuration object to fill
	 */
	void setRobotTurnConfigs(TalonFXInvertType masterInvertType, TalonFXConfiguration masterConfig) {
		/**
		 * Determine if we need a Sum or Difference.
		 * 
		 * The auxiliary Talon FX will always be positive
		 * in the forward direction because it's a selected sensor
		 * over the CAN bus.
		 * 
		 * The master's native integrated sensor may not always be positive when forward
		 * because
		 * sensor phase is only applied to *Selected Sensors*, not native
		 * sensor sources. And we need the native to be combined with the
		 * aux (other side's) distance into a single robot heading.
		 */

		/*
		 * THIS FUNCTION should not need to be modified.
		 * This setup will work regardless of whether the master
		 * is on the Right or Left side since it only deals with
		 * heading magnitude.
		 */

		/* Check if we're inverted */
		if (masterInvertType == TalonFXInvertType.Clockwise) {
			/*
			 * If master is inverted, that means the integrated sensor
			 * will be negative in the forward direction.
			 * If master is inverted, the final sum/diff result will also be inverted.
			 * This is how Talon FX corrects the sensor phase when inverting
			 * the motor direction. This inversion applies to the *Selected Sensor*,
			 * not the native value.
			 * Will a sensor sum or difference give us a positive heading?
			 * Remember the Master is one side of your drivetrain distance and
			 * Auxiliary is the other side's distance.
			 * Phase | Term 0 | Term 1 | Result
			 * Sum: -((-)Master + (+)Aux )| OK - magnitude will cancel each other out
			 * Diff: -((-)Master - (+)Aux )| NOT OK - magnitude increases with forward
			 * distance.
			 * Diff: -((+)Aux - (-)Master)| NOT OK - magnitude decreases with forward
			 * distance
			 */

			masterConfig.sum0Term = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); // Local Integrated
																								// Sensor
			masterConfig.sum1Term = TalonFXFeedbackDevice.RemoteSensor1.toFeedbackDevice(); // Aux Selected Sensor
			masterConfig.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SensorSum.toFeedbackDevice(); // Sum0
																													// +
																													// Sum1

			/*
			 * PID Polarity
			 * With the sensor phasing taken care of, we now need to determine if the PID
			 * polarity is in the correct direction
			 * This is important because if the PID polarity is incorrect, we will run away
			 * while trying to correct
			 * Will inverting the polarity give us a positive counterclockwise heading?
			 * If we're moving counterclockwise(+), and the master is on the right side and
			 * inverted,
			 * it will have a negative velocity and the auxiliary will have a negative
			 * velocity
			 * heading = right + left
			 * heading = (-) + (-)
			 * heading = (-)
			 * Let's assume a setpoint of 0 heading.
			 * This produces a positive error, in order to cancel the error, the right
			 * master needs to
			 * drive backwards. This means the PID polarity needs to be inverted to handle
			 * this
			 * 
			 * Conversely, if we're moving counterclwise(+), and the master is on the left
			 * side and inverted,
			 * it will have a positive velocity and the auxiliary will have a positive
			 * velocity.
			 * heading = right + left
			 * heading = (+) + (+)
			 * heading = (+)
			 * Let's assume a setpoint of 0 heading.
			 * This produces a negative error, in order to cancel the error, the left master
			 * needs to
			 * drive forwards. This means the PID polarity needs to be inverted to handle
			 * this
			 */
			masterConfig.auxPIDPolarity = true;
		} else {
			/* Master is not inverted, both sides are positive so we can diff them. */
			masterConfig.diff0Term = TalonFXFeedbackDevice.RemoteSensor1.toFeedbackDevice(); // Aux Selected Sensor
			masterConfig.diff1Term = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); // Local
																								// IntegratedSensor
			masterConfig.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SensorDifference
					.toFeedbackDevice(); // Sum0 + Sum1
			/*
			 * With current diff terms, a counterclockwise rotation results in negative
			 * heading with a right master
			 */
			masterConfig.auxPIDPolarity = true;
		}
		/**
		 * Heading units should be scaled to ~4000 per 360 deg, due to the following
		 * limitations...
		 * - Target param for aux PID1 is 18bits with a range of [-131072,+131072]
		 * units.
		 * - Target for aux PID1 in motion profile is 14bits with a range of
		 * [-8192,+8192] units.
		 * ... so at 3600 units per 360', that ensures 0.1 degree precision in firmware
		 * closed-loop
		 * and motion profile trajectory points can range +-2 rotations.
		 */
		masterConfig.auxiliaryPID.selectedFeedbackCoefficient = DriveTrainConstants.TURN_TRAVEL_UNITS_PER_ROTATION
				/ DriveTrainConstants.ENCODER_UNITS_PER_ROTATION;
	}

}