Autonomous Help

[curtis0gj]

Hey all, newbie programmer here looking for some help with my autonomous code. At the moment I need to be able to turn 90 on the spot using a gyro stop and drive straight also using a gyro and encoder for distance.

public void autonomous() {
    	
    	while(isAutonomous() && isEnabled())  {
    		
    		SmartDashboard.putNumber("angle", gyro1.getAngle());
    		
    		double angle = gyro1.getAngle();
    		
    		robot.drive(-0.20, 1);
    		
    		if(angle > 83) {
    			
    			robot.drive(0,0);
    			
    			break;

The issue I have is the robot won't stop turning . I need a basic tutorial on setting up an encoder for distance. Also if anyone could show me how I could start driving straight after this turn, Thanks for the help.

[curtis0gj]

Originally Posted by curtis0gj

Hey all, newbie programmer here looking for some help with my autonomous code. At the moment I need to be able to turn 90 on the spot using a gyro stop and drive straight also using a gyro and encoder for distance.

public void autonomous() {
    	
    	while(isAutonomous() && isEnabled())  {
    		
    		SmartDashboard.putNumber("angle", gyro1.getAngle());
    		
    		double angle = gyro1.getAngle();
    		
    		robot.drive(-0.20, 1);
    		
    		if(angle > 83) {
    			
    			robot.drive(0,0);
    			
    			break;

The issue I have is the robot won't stop turning . I need a basic tutorial on setting up an encoder for distance. Also if anyone could show me how I could start driving straight after this turn, Thanks for the help.

I think I have solved this but I don't have a way of testing it until tomorrow, I think the issue is the double angle = gyro1.getAngle(); is in the while loop so it's just getting the angle over and over... But I still need help with setting up an encoder if anyone could help that would be great!

[curtis0gj]

Originally Posted by curtis0gj

I think I have solved this but I don't have a way of testing it until tomorrow, I think the issue is the double angle = gyro1.getAngle(); is in the while loop so it's just getting the angle over and over... But I still need help with setting up an encoder if anyone could help that would be great!

Quoting my self for a third time lol.... I spent a bit more time trying to setup the straight driving and what I did was created a new boolean and set it to true and made an if loop that it will enter if its == to true.

public void autonomous() {
    	
		double angle = gyro1.getAngle();
		
    	while(isAutonomous() && isEnabled())  {
    		
    		SmartDashboard.putNumber("angle", gyro1.getAngle());
    		
    		robot.drive(-0.20, 1);
    		
    		if(angle > 83) {
    			
    			robot.drive(0,0);
    			
    			break; 
    			
    			
    			}
    		
    		//do i need to reset my gryo? do i need to place my get angle here? 
    		

    			boolean turnCompleted = true;

				if(turnCompleted == true) {
					
					double angle2 = gyro1.getAngle(); // i think for driving i need a constant update of my angle
					//so i will place this inside my if loop and i name it angle2 so it's a different variable.
					robot.drive(-1.0, -angle2*Kp); // driving straight in a line.
					Timer.delay(0.004);
					
			
    			}
				
    		}
    		}

If anyone has any ideas for improvement for this please let me know.

[notmattlythgoe]

I think I can give you a hand Curtis. Could you please list step by step what it is you want your autonomous mode to do?

[curtis0gj]

Originally Posted by notmattlythgoe

I think I can give you a hand Curtis. Could you please list step by step what it is you want your autonomous mode to do?

Hi, thanks for the reply so basically what I am looking to do is make a 180 degree turn stop and drive straight forward for a set distance via encoder. However our team only has one encoder currently I don't know if this will be a problem. Today I figured out the turn finally and I added in the straight away however I am stuck in a loop I believe here's my current program

    public void autonomous() {
    	
    	gyro1.reset();
		
    	while(isAutonomous() && isEnabled())  {
    		
    		double angle = gyro1.getAngle();
    		
    		SmartDashboard.putNumber("angle", gyro1.getAngle());
    		
    		robot.drive(-0.15, 1);
    		Timer.delay(0.1);
    		
    		if(angle > 160) {
    			
    			robot.drive(0,0);
    			gyro1.reset();
    			Timer.delay(1.00);
    			
    			turnComplete = false;
    			break; 
    			
    			
    			
    			
    			}
    		}
    		turnComplete = true;
				gyro1.reset();
				while (turnComplete == true) {

						
						double angle2 = gyro1.getAngle(); // i think for driving i need a constant update of my angle
						//so i will place this inside my if loop and i name it angle2 so it's a different variable.
						robot.drive(-0.15, -angle2 * Kp); // driving straight in a line.
						Timer.delay(0.004);
						
						if(isAutonomous() && isDisabled())  {
							robot.drive(0,0);
							turnComplete = false;
							break;
						}
					
				
				}
				
				
			
    			}

[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.

[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!

[notmattlythgoe]

Originally Posted by curtis0gj

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!

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:

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:

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.

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.

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.

[curtis0gj]

Originally Posted by notmattlythgoe

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:

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:

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.

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.

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.

Would the if statement be if(encoder.getDistance > 50) work at all?

[notmattlythgoe]

Originally Posted by curtis0gj

Would the if statement be if(encoder.getDistance > 50) work at all?

Add parameters to your method call and it's perfect.

[curtis0gj]

Originally Posted by notmattlythgoe

Add parameters to your method call and it's perfect.

I think I have this right but before we wrap this up I just wanted to check a few things. Do I need < , > or ==. Also I would like to print a distance traveled to my smart DS is that just SmartDashboard.putNumber("Distance", encoder.getDistance()); or can I make a double distance = encoder.getDistance(); and just put distance in there?? Here's my code so far.

public class Robot extends IterativeRobot {
	
	RobotDrive robot;
    Joystick stick;
    Encoder encoder;	
    Gyro gyro1;
    
    static final double Kp = 0.05;
    
    public void robotInit() {
    	
    	 robot = new RobotDrive(0, 1);
         stick = new Joystick(0);
         gyro1 = new Gyro(0);
         gyro1.reset();
         gyro1.initGyro();
         Encoder encoder = new Encoder(1, 2, false, EncodingType.k4X);
         
         encoder.setDistancePerPulse(0.001);
         encoder.getDistance(); // does this belong in the init or my if loop?

    }

   
    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); 
					// This can't be right lol..... at least its no longer dead code (: also in my if statement do i need < or > or ==?
						if(encoder.getDistance() > 50)  {
							robot.drive(0,0); 
							
							break;
						}
						Timer.delay(0.1);	
				}
				
    			}

[notmattlythgoe]

Originally Posted by curtis0gj

I think I have this right but before we wrap this up I just wanted to check a few things. Do I need < , > or ==. Also I would like to print a distance traveled to my smart DS is that just SmartDashboard.putNumber("Distance", encoder.getDistance()); or can I make a double distance = encoder.getDistance(); and just put distance in there?? Here's my code so far.

public class Robot extends IterativeRobot {
	
	RobotDrive robot;
    Joystick stick;
    Encoder encoder;	
    Gyro gyro1;
    
    static final double Kp = 0.05;
    
    public void robotInit() {
    	
    	 robot = new RobotDrive(0, 1);
         stick = new Joystick(0);
         gyro1 = new Gyro(0);
         gyro1.reset();
         gyro1.initGyro();
         Encoder encoder = new Encoder(1, 2, false, EncodingType.k4X);
         
         encoder.setDistancePerPulse(0.001);
         encoder.getDistance(); // does this belong in the init or my if loop?

    }

   
    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); 
					// This can't be right lol..... at least its no longer dead code (: also in my if statement do i need < or > or ==?
						if(encoder.getDistance() > 50)  {
							robot.drive(0,0); 
							
							break;
						}
						Timer.delay(0.1);	
				}
				
    			}

> will work perfectly fine.

AS for the distance variable, I would create a variable just like you did with the angle and print it to the SmartDashboard.

[curtis0gj]

This quote thing is driving me nuts, anyway this works right?

double angle = gyro1.getAngle();
    		double distance = encoder.getDistance();
    		
    		SmartDashboard.putNumber("Angle", angle);
    		SmartDashboard.putNumber("Distance", distance);

Also now that I have made my distance variable can I use it in my if statement like so, if(distance > 50) ?

[notmattlythgoe]

Originally Posted by curtis0gj

This quote thing is driving me nuts, anyway this works right?

double angle = gyro1.getAngle();
    		double distance = encoder.getDistance();
    		
    		SmartDashboard.putNumber("Angle", angle);
    		SmartDashboard.putNumber("Distance", distance);

Also now that I have made my distance variable can I use it in my if statement like so, if(distance > 50) ?

Yes you can, but you are going to want to make sure that you put the distance variable in the second loop so it gets updated when you are driving.

Another thing I forgot to have you do is reset the encoder prior to the second loop. You can do this in the same place you reset the gyro.

encoder.reset();

[curtis0gj]

Originally Posted by notmattlythgoe

Yes you can, but you are going to want to make sure that you put the distance variable in the second loop so it gets updated when you are driving.

Another thing I forgot to have you do is reset the encoder prior to the second loop. You can do this in the same place you reset the gyro.

encoder.reset();

I think this is perfect now I am very excited to test this.

public void autonomousPeriodic() {

    	gyro1.reset();
		
    	while(isAutonomous() && isEnabled())  {
    		
    		double angle = gyro1.getAngle();
    		double distance = encoder.getDistance();
    		
    		SmartDashboard.putNumber("Angle", angle);
    		SmartDashboard.putNumber("Distance", distance);
    		
    		robot.drive(-0.25, 1);
    		
    		if(angle > 150) {
    			
    			robot.drive(0,0);
    			gyro1.reset();
    			encoder.reset();
    			
    			Timer.delay(1.00);

    			break; 
    			
    			}
    		
    			Timer.delay(0.1);
    	}
    	
				while (isAutonomous() && isEnabled()) {
					
					double distance2 = encoder.getDistance();

					double angle2 = gyro1.getAngle(); 
						
					robot.drive(-0.20, -angle2 * Kp); 
					
						if(distance2 > 50)  {
							robot.drive(0,0); 
							
							break;
						}
						Timer.delay(0.1);	
				}
				
    			}