Move Timer.delay(0.1) down below the first if statement. Also add another one below the second if statement. It should look like this:
Code:
public void autonomousPeriodic() {
gyro1.reset();
while(isAutonomous() && isEnabled()) {
double angle = gyro1.getAngle();
SmartDashboard.putNumber("angle", angle);
robot.drive(-0.25, 1);
if(angle > 150) {
robot.drive(0,0);
gyro1.reset();
Timer.delay(1.00);
break;
}
Timer.delay(0.1);
}
while (isAutonomous() && isEnabled()) {
double angle2 = gyro1.getAngle();
robot.drive(-0.20, -angle2 * Kp);
if(false) {
robot.drive(0,0); //It's calling all this dead code ):
break;
}
Timer.delay(0.1);
}
}
You are correct in your comment, that section of code is unreachable right now. Once we add in an encoder it will get fixed.
Do create an encoder you'll do something like this:
Code:
Encoder encoder = new Encoder(1, 2, false, EncodingType.k4X);
The first 2 parameters are the digital channels the encoder is plugged into. The next parameter will allow you to reverse the direction of the encoder if you decide that you want it to count in the opposite direction. The 4th parameter is the type of encoder you are using. For this case I'm going to assume you are using a quadrature encoder.
Next lets configure the encoder. Configuring an encoder correctly will make your life that much easier when you want to use it.
Code:
encoder.setDistancePerPulse(0.001);
Setting a value in this method will allow you to get readings from the encoder that actually mean something. Figure out how far your robot moves in one rotation of the encoder, then divide that distance by the number of counts your encoder has.
Then, to get the reading all you have to do is call the below method, this method will return how far the encoder has. If you have configured the encoder with the correct distance per pulse then it will return how far your robot has moved.
Code:
encoder.getDistance();
Now that we have an encoder we can plug it into your code. Replace the false with a check to see if you have traveled the distance you want.