package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.FirebotGyro;
+import org.usfirst.frc.team3501.robot.Lidar;
+import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.CANTalon;
-import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
// Drivetrain related objects
private Encoder leftEncoder, rightEncoder;
+ public static Lidar lidar;
private CANTalon frontLeft, frontRight, rearLeft, rearRight;
private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC;
// Drivetrain specific constants that relate to the inches per pulse value for
private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
public final static double INCHES_PER_PULSE = (((Math.PI)
- * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
- * WHEEL_DIAMETER;
+ * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
+ / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+
// Drivetrain specific constants that relate to the PID controllers
- private final static double Kp = 1.0, Ki = 0.0, Kd = 0.0;
+ private final static double Kp = 1.0, Ki = 0.0,
+ Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
+ / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+
+ public AnalogInput channel;
+
+ // Gyro stuff
+ private final static double NANOSECONDS_PER_SECOND = 1000000000;
+ short rawValue;
+ public FirebotGyro gyro;
+
+ double initialSpeedNanoseconds;
+ double finalSpeedNanoseconds;
+ double initialSpeedSeconds;
+ double finalSpeedSeconds;
+ double deltaSpeed;
+ double degrees;
public DriveTrain() {
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
- this.configureTalons();
-
- frontLeftC = new PIDController(Kp, Ki, Kd, frontLeft, frontLeft);
- frontRightC = new PIDController(Kp, Ki, Kd, frontRight, frontRight);
- rearLeftC = new PIDController(Kp, Ki, Kd, frontLeft, rearLeft);
- rearRightC = new PIDController(Kp, Ki, Kd, frontRight, rearRight);
- this.enablePIDControllers();
+ lidar = new Lidar(I2C.Port.kOnboard);
leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
+ leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+ rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+
+ gyro = new FirebotGyro(I2C.Port.kOnboard, (byte) 0x68);
+ gyro.initialize();
}
@Override
protected void initDefaultCommand() {
}
- public void configureTalons() {
- frontLeft.changeControlMode(TalonControlMode.Speed);
- frontRight.changeControlMode(TalonControlMode.Speed);
- rearLeft.changeControlMode(TalonControlMode.Speed);
- rearRight.changeControlMode(TalonControlMode.Speed);
-
- frontLeft.configEncoderCodesPerRev(256);
- frontRight.configEncoderCodesPerRev(256);
- rearLeft.configEncoderCodesPerRev(256);
- rearRight.configEncoderCodesPerRev(256);
-
- frontLeft.enableControl();
- frontRight.enableControl();
- rearLeft.enableControl();
- rearRight.enableControl();
- }
-
- public void enablePIDControllers() {
- frontLeftC.enable();
- frontRightC.enable();
- rearLeftC.enable();
- rearRightC.enable();
- }
-
- public void drive(double left, double right) {
- frontLeftC.setSetpoint(left);
- rearLeftC.setSetpoint(left);
- frontRightC.setSetpoint(right);
- rearRightC.setSetpoint(right);
- }
-
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
}
+ public double getLidarDistance() {
+ return lidar.pidGet();
+ }
+
public double getRightSpeed() {
return rightEncoder.getRate(); // in inches per second
}