import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.GyroLib;
+import org.usfirst.frc.team3501.robot.Lidar;
import org.usfirst.frc.team3501.robot.MathLib;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
public class DriveTrain extends PIDSubsystem {
- // Encoder PID Proportional Constants P, I, and D
- private static double EP = 0.013, EI = 0.000015, ED = -0.002;
-
- // Gyro PID Constants P, I, and D
- private static double GP = 0.018, GI = 0.000015, GD = 0;
+ private static double kp = 0.013, ki = 0.000015, kd = -0.002;
+ private static double gp = 0.018, gi = 0.000015, gd = 0;
private static double pidOutput = 0;
// PID Controller tolerances for the error
private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
private Encoder leftEncoder, rightEncoder;
+
+ public static Lidar leftLidar;
+ public static Lidar rightLidar;
+
private CANTalon frontLeft, frontRight, rearLeft, rearRight;
private RobotDrive robotDrive;
rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ leftLidar = new Lidar(I2C.Port.kOnboard);
+ rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
+ // lidar
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.LEFT_REVERSE);
rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
Constants.DriveTrain.RIGHT_REVERSE);
+ Constants.DriveTrain.inverted = false;
}
@Override
rightEncoder.reset();
}
+ public double getLeftLidarDistance() {
+ return leftLidar.pidGet();
+ }
+
+ public double getsRightLidarDistance() {
+ return rightLidar.pidGet();
+ }
+
public double getRightSpeed() {
return rightEncoder.getRate(); // in inches per second
}
/*
* Method is a required method that the PID Subsystem uses to return the
* calculated PID value to the driver
- *
+ *
* @param Gives the user the output from the PID algorithm that is calculated
* internally
- *
+ *
* Body: Uses the output, does some filtering and drives the robot
*/
@Override
output = Math.signum(output) * 0.3;
left = output;
right = output + drift * kp / 10;
- }
- else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+ } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
left = output;
right = -output;
}
/*
* Checks the drive mode
- *
+ *
* @return the current state of the robot in each state
* Average distance from both sides of tank drive for Encoder Mode
* Angle from the gyro in GYRO_MODE
public void drive(double left, double right) {
robotDrive.tankDrive(-left, -right);
// dunno why but inverted drive (- values is forward)
+ if (!Constants.DriveTrain.inverted)
+ robotDrive.tankDrive(-left,
+ -right);
+ else
+ robotDrive.tankDrive(right, left);
}
/*
* constrains the distance to within -100 and 100 since we aren't going to
* drive more than 100 inches
- *
+ *
* Configure Encoder PID
- *
+ *
* Sets the setpoint to the PID subsystem
*/
public void driveDistance(double dist, double maxTimeOut) {
/*
* Turning method that should be used repeatedly in a command
- *
+ *
* First constrains the angle to within -360 and 360 since that is as much as
* we need to turn
- *
+ *
* Configures Gyro PID and sets the setpoint as an angle
*/
public void turnAngle(double angle) {