Greetings,
Our robot does not drive straight with the current code we have. We are using different methods to make it drive straight with encoders and gyros. However, these methods are causing it to veer to one side.
Code:
public class AutonomousDrive extends CommandBase {
double left = .5;
double right = .475;
public AutonomousDrive() {
requires(driveTrain);
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
}
// Called just before this Command runs the first time
protected void initialize() {
driveTrain.resetEncoder();
driveTrain.gyro.reset();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
driveTrain.driveXboxTank(left, right);
double rightEncoderCompensation = driveTrain.leftEncoder.getDistance() - driveTrain.rightEncoder.getDistance();
double increaseFactor = rightEncoderCompensation/200.0;
if (rightEncoderCompensation > 0)
{
if (right >= 0.5) {
left -= increaseFactor;
} else {
right += increaseFactor;
}
} else if (rightEncoderCompensation < 0) {
if (left >= 0.5) {
right -= increaseFactor;
} else {
left += increaseFactor;
}
}
/*if (driveTrain.gyro.getAngle() > 0.5) {
if (right == 1.0) {
left -= .0025;
} else {
right += .0025;
}
} else if (driveTrain.gyro.getAngle() < -0.5) {
if (left == 1.0) {
right -= .0025;
} else {
left += .0025;
}
}*/
}
We were thinking maybe PID is the solution, but we have no idea on how to implement it.