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,
rightEncoder.reset();
}
+ public double getLeftLidarDistance() {
+ return leftLidar.pidGet();
+ }
+
+ public double getRightLidarDistance() {
+ return rightLidar.pidGet();
+ }
+
public double getRightSpeed() {
return rightEncoder.getRate(); // in inches per second
}
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;
}