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

    // Uncomment next 2 lines for PWM motor controllers (Macademia)
    // private Talon leftMotor, rightMotor;
    // private Encoder leftEncoder, rightEncoder;
  
    // Uncomment next line for CAN motor controllers (Hazelnut)
    // private WPI_TalonSRX leftMotor, rightMotor;
  

• In robotInit() instantiate the Encoder and Motor objects
  • For PWM Motor Controller robots (Macadamia):

      // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3
      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);
      // PWM motor controllers connected to PWM0, PWM1
      leftMotor = new Talon(0);
      rightMotor = new Talon(1);
    

  • For CAN Motor Controller robots (Hazelnut):

      // CAN motor controllers connected to CAN bus (addrs 3,4)
      leftMotor = new WPI_TalonSRX(3);
      rightMotor = new WPI_TalonSRX(4);
      // clear any previously configured settings
      leftMotor.configFactoryDefault();
      rightMotor.configFactoryDefault();
      // Encoders are connected directly to the motor controllers
      leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
      rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
    

• In robotPeriodic() display the encoder values
  • For PWM controller robots (Macadamia):

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

  • For CAN motor controllers (Hazelnut) note the use of the ​getSelectedFeedbackSensor() method of the motor controller object which returns raw encoder counts.
    • Add the following function to the Robot class:

         double raw2inches(int raw_count, int counts_per_rotation, double wheel_diameter) {
            return (raw_count / counts_per_rotation) * (3.141492 * wheel_diameter);
         }
      

    • Add the following to the robotPeriodic() method:

         // Encoders connected to CAN controller return raw counts
         double l_raw = leftMotor.getSelectedSensorPosition();
         double r_raw = rightMotor.getSelectedSensorPosition();
         // display distances on smart dashboard
         SmartDashboard.putNumber("Left", raw2inches(l_raw, 360*2, 7.5));
         SmartDashboard.putNumber("Right", raw2inches(r_raw, 360*2, 7.5));
      

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

    // For PWM controllers (Macadamia) uncomment the next 2 lines to 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
  • For PWM motor controllers:

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

    • For CAN motor controllers:

           double l_raw = leftMotor.getSelectedSensorPosition();
           double r_raw = rightMotor.getSelectedSensorPosition();
           if ((raw2inches(l_raw, 360*2, 7.5) > 36.0) || 
               (raw2inches(r_raw, 360*2, 7.5) > 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.