Autonomous

In the previous examples, we learned to apply power to the wheel motors and to see how far the wheel has turned using encoders. Now we're going to put them together to drive the robot a controlled distance. We're also going to learn about autonomous mode. Every FRC match starts with a 15 second autonmous period where the robot drives itself and tries to accomplish some task with no human driver. For this example, our task is to drive 3 feet forward and then stop (harder than it sounds).

Create another program using the TimedRobot java template and name it AutonomousTest1. Modify the generated code as follows:

• Add the following imports:

  import edu.wpi.first.wpilibj.Encoder;
  import com.ctre.phoenix.motorcontrol.can.*; // for Hazelnut
  import edu.wpi.first.wpilibj.Talon;         // for Macadamia
  

• Declare variables for the Encoders and Motor controllers in the Robot class

    private Encoder leftEncoder, rightEncoder;
  
    // Uncomment next line for Macademia
    // private Talon leftMotor, rightMotor;
    // Uncomment next line for Hazelnut
    // private WPI_TalonSRX leftMotor, rightMotor;
  

• In robotInit() instantiate the Encoder and Motor objects

    // Quadrature encoder has A and B channels connected to DIO0, DIO1
    leftEncoder = new Encoder(0, 1);
    rightEncoder = new Encoder(2, 3);
    // Peanut wheels are 7.5" in diameter
    // Encoders provide 1 count per degree of rotation
    leftEncoder.setDistancePerPulse((3.141592*7.5)/360);
    rightEncoder.setDistancePerPulse((3.141592*7.5)/360);
  
    // Uncomment the next two lines for Macademia
    // leftMotor = new Talon(0);
    // rightMotor = new Talon(1);
  
    // uncoment the next two lines for Hazelnut
    // leftMotor = new WPI_TalonSRX(3);
    // rightMotor = new WPI_TalonSRX(4);
  

• In robotPeriodic() display the encoder values

    SmartDashboard.putNumber("Left", leftEncoder.getDistance());
    SmartDashboard.putNumber("Right", rightEncoder.getDistance());
  

• In autonomousInit() reset the encoder counts and start the motors:

    // reset encoder counts to 0
    leftEncoder.reset();
    rightEncoder.reset();
    // start motors turning forward at 20% power
    leftMotor.set(0.20);
    // to go forward, left motor turns clockwise, right motor counter-clockwise
    rightMotor.set(-0.20);
  

• In the default section of autonomousPeriodic() stop if we've gone the desired distance. Notice our first use of the logical OR operator (||) to test whether either of two conditions is true

        if ((leftEncoder.getDistance() > 36.0) || 
            (rightEncoder.getDistance() > 36.0)) {
           leftMotor.stopMotor();
           rightMotor.stopMotor();
        }
  

When you launch the driver station, you'll need to select "Autonomous" mode (look above the Enable button) and then press the Enable button. The autonomousInit() method will run first and then the autonomousPeriodic() method will be called repeatedly until the robot is disabled. If your robot isn't doing what it should be, press the Disable button immediately to stop the robot. It's usually a good idea to test your code with the robot up on a stool or blocks (wheels not touching the ground) in case your code doesn't work properly.

Why didn't the robot go straight?

Extra credit: See if you can make it go straight.