Autonomous Help

[curtis0gj]

Originally Posted by notmattlythgoe

Curtis,

You have indeed made some progress, let's go through your program and talk about what is going on.

First, at the beginning of the autonomous period the autonomous() method is called. Anything in this method will run during the autonomous period.

public void autonomous() {
...
}

There are 2 routes you can take to program this:

[1] You can create different loops that do different things and as one loop finishes the next loop starts. This is the route that you have taken, so I will help you finish this.

[2] You can create one loop that has different checks in it to determine what to do each time.

What we you will want to do is have a loop that looks like the below for each stage of your autonomous period. If you do not check if autonomous mode is running and that you are enabled then you run the risk of getting stuck in autonomous for the entire match.

while(isAutonomous() && isEnabled())  {
...

Let's examine your first loop, which you are using to turn 180 degrees:

while(isAutonomous() && isEnabled())  {
    		
[1]	double angle = gyro1.getAngle();
    		
[2]	SmartDashboard.putNumber("angle", gyro1.getAngle());
    		
[3]	robot.drive(-0.15, 1);
[4]	Timer.delay(0.1);
    		
[5]	if(angle > 160) {
    			
[5a]		robot.drive(0,0);
[5b]		gyro1.reset();
[5c]		Timer.delay(1.00);
    			
[5d]		turnComplete = false;
[5e]		break; 
	}
}

Each time the loop executes it follows the following logic while the robot is in the autonomous period:
[1] Get the current gyro angle
[2] Put the current gyro angle on the SmartDashboard
[3] Drive backward at 15% speed and spin at full speed
[4] Delay the loop code for 0.1 seconds
[5] If the angle is > 160 do the folowing
[5a] Stop the robot
[5b] Reset the gyro
[5c] Delay the loop code for 1 second
[5d] Set turnComplete to false
[5e] Break the current loop and continue on with the code

[1] is done correctly.
[2] is also working, but since you have the angle already, it is better convention to use the variable already holding it. Let's change this line to:

SmartDashboard.putNumber("angle", angle);

[3] will work, but the number seem odd to me. I'd need more information about your robot setup to direct you to the proper numbers.
[4] works, but let's move it the end of the loop instead. Place it right below the closing } for your if statement.
[5], [5a], and [5b] are perfect
[5c] I'm assuming you are wanting to pause for a second after you finish your turn. If this is the case this is fine where it is.
[5d] I'm going to get you to remove this line, we won't need it later.
[5e] is also perfect

Now, let's look at your next loop:

[1]	turnComplete = true;
[2]	gyro1.reset();
[3]	while (turnComplete == true) {

[4]		double angle2 = gyro1.getAngle();

[5]		robot.drive(-0.15, -angle2 * Kp); // driving straight in a line.
[6]		Timer.delay(0.004);
							
[7]		if(isAutonomous() && isDisabled())  {
[7a]		robot.drive(0,0);
[7b]		turnComplete = false;
[7c]		break;
		}
	}

[1] We're not going to use this because it is unneeded, so let's delete this line.
[2] You actually already did this in the loop above, so you can either remove this line, or the identical line in the other loop.
[3] Let's make this loop run with the same check as the other loop. The reason why is if the autonomous mode ends and this loop hasn't finished then it will automatically finish.
[4] This line works just fine.
[5] This should work to keep you moving in a straight line, you'll just need to tune the Kp value to get it perfect.
[6] Any reason you are waiting for 0.004 seconds?
[7] We will change this line later to read the encoder distance and determine if we need to stop. For now let's make it:

if (false) {

[7a] This is good.
[7b] Let's remove this since we aren't using it.
[7c] This is also good

Let me know if you have any questions about what I've said so far.

So far so good thanks a ton for the help here's what I have right now with these fixes

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);

    			Timer.delay(0.1);
    			break; 
    			
    			}
    	}
    		
				
				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;
						}
					
				}
				
				
    			}

Now I just need some help with that encoder and that would be perfect, thanks a ton!