Merge pull request #1 from FRCTeam1024/Projects

[1024Programming]

// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

package frc.robot.subsystems;

import frc.robot.commands.*;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.AnalogGyro;
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.wpilibj.SerialPort;
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.CounterBase.EncodingType;

import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS

/**
*
*/
public class DriveTrain extends Subsystem {

// public Encoder encoder = new Encoder(0, 1, false, EncodingType.k4X);

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
private WPI_TalonSRX frontRight;
private WPI_TalonSRX middleRight;
private WPI_TalonSRX rearRight;
public WPI_TalonSRX frontLeft; // This is being used as the encoder
private WPI_TalonSRX middleLeft;
private WPI_TalonSRX rearLeft;
private AnalogGyro navx;
private I2C i2c;
public AHRS ahrs;

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

public DriveTrain() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    frontRight = new WPI_TalonSRX(5);

    middleRight = new WPI_TalonSRX(8);

    rearRight = new WPI_TalonSRX(9);

    frontLeft = new WPI_TalonSRX(2);

    middleLeft = new WPI_TalonSRX(3);

    rearLeft = new WPI_TalonSRX(4);

    // private final I2C.Port i2cPort = I2C.Port.kMXP;
    // navx = new AnalogGyro(Port.kMXP);
    ahrs = new AHRS(SerialPort.Port.kMXP);
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    //encoder.setDistancePerPulse((1.0 / 71.0) * 4.0);
}

@Override
public void initDefaultCommand() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND

    setDefaultCommand(new DriveWithJoysticks());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND

    // Set the default command for a subsystem here.
    // setDefaultCommand(new MySpecialCommand());
}

@Override
public void periodic() {
    // Put code here to be run every loop
    outputToSmartDashboard();
    //SmartDashboard.putBoolean("Reset Encoder", resetOpticalEncoder());
}

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS

public void drive(double leftPower, double rightPower) {
    frontLeft.set(ControlMode.PercentOutput, leftPower);
    middleLeft.set(ControlMode.PercentOutput, leftPower);
    rearLeft.set(ControlMode.PercentOutput, leftPower);
    frontRight.set(ControlMode.PercentOutput, -rightPower);
    middleRight.set(ControlMode.PercentOutput, -rightPower);
    rearRight.set(ControlMode.PercentOutput, -rightPower);
}
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS

// Put methods for controlling this subsystem
// here. Call these from Commands.
/**public double getRawRightEncoder() {
    outputToSmartDashboard();
	return encoder.get();

}**/

public double getOpticalDistanceInches() {

	/*//getWheelRotation() = distance / (Math.PI * RobotMap.WHEEL_DIAMETER) * RobotMap.ENCODER_COUNTS_PER_REVOLUTION;

	return (((getRawMagneticEncoder() / RobotMap.ENCODER_RATIO_TO_WHEEL * Math.PI * RobotMap.WHEEL_DIAMETER) / 

			RobotMap.OPTICAL_ENCODER_COUNTS_PER_REVOLUTION)/ 4) * 3 * 3;*/

    //return encoder.getDistance(); //must have a specified distance per pulse set
    return frontLeft.getSelectedSensorPosition() * 1/70; //(1.0 / 71.0) * 4.0;
    //wheel diameter: 6 in
}
public void resetOpticalEncoder() {

	frontLeft.setSelectedSensorPosition(0);

}

public void outputToSmartDashboard() {

	SmartDashboard.putNumber("Optical Encoder Distance (IN)", -getOpticalDistanceInches());
    SmartDashboard.putNumber("Optical Encoder Raw", -frontLeft.getSelectedSensorPosition());
    SmartDashboard.putNumber("IMU_CompassHeading", ahrs.getCompassHeading());
    SmartDashboard.putNumber("IMU_Yaw", ahrs.getYaw());

}
public void stop() {

	frontLeft.set(ControlMode.PercentOutput, 0.0);
    middleLeft.set(ControlMode.PercentOutput, 0.0);
    rearLeft.set(ControlMode.PercentOutput, 0.0);
    frontRight.set(ControlMode.PercentOutput, 0.0);
    middleRight.set(ControlMode.PercentOutput, 0.0);
    rearRight.set(ControlMode.PercentOutput, 0.0);

}

}