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 {
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
+ // the encoders
+ private final static double WHEEL_DIAMETER = 6.0; // in inches
+ private final static double PULSES_PER_ROTATION = 256; // in pulses
+ 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;
+ // Drivetrain specific constants that relate to the PID controllers
+ 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 DriveTrain() {
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
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);
+
}
@Override
this.rearLeft.set(leftSpeed);
this.rearRight.set(-rightSpeed);
}
+
}