I hope that it is ok to jump in here...
It looks like you started off with a Robot that extended SampleRobot and have now migrated to a class that extends IterativeRobot. The way that your autonomous code will be executed is very different in these two cases.
SampleRobot
When you extend SampleRobot, your autonomous method will get called for you once. You need to write loops in your code that keep running until you have completed your task. This is why you have the while loop:
while( isAutonomous() and isEnabled() )
{
//stuff
}
You also put your own calls to Timer.delay() in this type of robot because nothing is pausing your code (nothing is creating a timed structure for you).
IterativeRobot
When you extend IterativeRobot, your autonomousPeriodic() method will be called
every 20 ms. If you put a loop in this method that runs for longer than 20 ms...well, that is bad. You also don't want to put calls to Timer.delay() in here as this will slow down the execution cycle. Since the code in autonomousPeriodic() is being executed every 20 ms, you want to avoid loops. Put the body of your loops in the method; since it is being executed every 20 ms, it is being looped for you. Also, be careful about resetting values in autonomousPeriodic() without checking a condition. Since the code gets executed repeatedly, you will be continuously resetting the value (e.g. the call to reset the gyro at the top of the method is constantly resetting the gyro).
Proposed new method:
Code:
//a new instance variable for the class...true until the initial turn is done...only false when you want to drive straight
boolean doingInitialTurn;
public void autonomousInit() {
gyro1.reset();
doingInitialTurn = true;
}
public void autonomousPeriodic() {
double angle = gyro1.getAngle();
double distance = encoder.getDistance(); //NOTE: changed to getDistance()
SmartDashboard.putNumber("angle", angle);
SmartDashboard.putNumber("distance", distance);
// if you are just starting and doing the first turn...turn code
if( angle <= 150 && doingInitialTurn )
robot.drive(-0.15, -1);
// otherwise, if the angle is now greater than 150 but you are just finishing the turn...stop turning
else if( doingInitialTurn ) {
robot.drive(0,0);
doingInitialTurn = false;
gyro1.reset();
encoder.reset();
}
// if you are completely done with the turn...drive forward instead
else
{
// you can reuse the original angle and distance values from the top of the method...
// they get updated every 20 ms
//double angle2 = gyro1.getAngle();
//double distance2 = encoder.getDistance();
// You also don't need to put them to the SmartDashboard again since the lines at the
// top of the method do that for you every 20 ms
//SmartDashboard.putNumber("distance", distance);
//SmartDashboard.putNumber("angle", angle);
robot.drive(-0.20, -angle * Kp);
if(distance > 100)
robot.drive(0,-0);
}
}
Also, the line of code that checks for when to stop driving forward had reversed logic; if the distance traveled was less than 100, you stopped driving. You want to stop driving only after the distance traveled is greater than 100. You had:
Lastly, you do not want to set the distance per pulse to be 1. The distance per pulse is the number of inches (or other unit) that the robot actually moves every time the encoder registers a "pulse", which is 250 times per revolution of the encoder (or 1000 times per revolution if you are using 4x encoding?). The Encoder's get() method returns the count (the same as the number of pulses?) since the last call to reset. The getDistance() method returns the count multiplied by the number specified in setDistancePerPulse(). If you set the distance per pulse to 1, then you will need to travel a great "distance" in your code.
To find the correct value for setDistancePerPulse(), we typically:
- Reset the encoder in teleopInit(). This code will only run when you first enable teleop.
- In teleopPeriodic(), get the count with encoder.get() and print it or put it up on the SmartDashboard. This code will run every 20 ms when teleop mode is enabled.
- Manually move the robot forward 10 feet (as close to 120 inches as you can).
- Divide 120 inches by the count from the encoder...that is your distance per pulse.
Good luck and let me know if this did or didn't work...unfortunately, I didn't test the code
