wiki:ControlSystems/SoftwareTeam/Training/GettingStarted/IntroRobotJava

Context Navigation

Version 37 (modified by David Albert, 6 years ago) (diff)
--

Java for FRC Robotics

Writing Java programs to control a robot is similar to the Java programming you've already learned; there are three key differences:

The program you write must be transferred to the robot's computer (the roboRIO) where it will run
FIRST and WPI provide a framework that your robot program must run within; the framework manages many aspects of the robot for you.
WPI also provides a rich library of pre-written classes called WPIlib that makes robot programming faster and easier.

Fifth program: Encoder

In the example above, we controlled the amount of power we were delivering the motor (0=none, 1.0=full power), but that doesn't tell us how fast the robot is moving or the wheel is turning. Just like a car or a bicycle, the speed a robot will go under a given amount of power depends on how heavy the robot is, whether it's going up a hill or down a hill, etc. To tell how fast a wheel is turning or to determine exactly where it has turned, we use encoders. An encoder is a device attached to the motor or wheel shaft that generates a pulse with each partial rotation (e.g. every 5-degrees of rotation, another pulse is generated). By counting these pulses, we can tell how fast the wheel is turning or what position it is in. You can read more about encoders here.

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

Add the following imports:
```
import edu.wpi.first.wpilibj.Encoder;
```
Declare a variable for the Encoder in the Robot class
```
  private Encoder leftEncoder;
```

• In robotInit() instantiate the Encoder object and reset it

  // Quadrature encoder has A and B channels connected to DIO0, DIO1
  leftEncoder = new Encoder(0, 1);
  // Peanut wheels are 7.5" in diameter
  // Encoders provide 1 count per degree of rotation
  leftEncoder.setDistancePerPulse((3.141592*7.5)/360);
  // reset encoder counts to 0
  leftEncoder.reset();

In robotPeriodic() read and display the encoder valuey

  int encoderValue = leftEncoder.getRaw();
  double distance = leftEncoder.getDistance();
  SmartDashboard.putNumber("Encoder Value", encoderValue);
  SmartDashboard.putNumber("Encoder Distance", distance);

Note: you'll need to manually turn the left wheel forward and backward to see the Encoder values change. Try turning the wheel 360 degrees to see what value is equivalent to a full revolution of the wheel. Notice that the raw count reports the sum of both A and B channels of the quadrature encoder.

Sixth program: Autonomous

In the examples above, 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 just to drive 3 feet 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.

Why didn't the robot go straight?

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

Seventh program: Closed Loop Control

In the last example, we saw why robot programming is challenging: robots work in the real world where wheels slip, motors have different powers, wheels aren't inflated equally, and floors aren't perfectly level. The robot didn't go straight and didn't stop at exactly 36". What makes robots interesting and powerful is their ability to sense their environment and respond to it. In the basic Autonomous example, one wheel was turning faster than the other so the robot turned. Since the encoders were reporting this, we can modify our program to compensate. The simplest way to do this is to proportionally increase the motor power of the wheel that's going slower.

MODIFY the last program as follows:

Add the following imports:
```
  import edu.wpi.first.wpilibj.Timer;
```

Add variables for closed loop control in the Robot class

  private double  leftPower, rightPower;
  private Timer   autoTimer;
  private boolean autoDone;

In robotInit() instantiate the new Timer object
```
  autoTimer = new Timer();
```

In robotPeriodic() add the motor power settings to the smart dashboard

  SmartDashboard.putNumber("Left power", leftPower);
  SmartDashboard.putNumber("Right power", rightPower);

In autonomousInit() initialize the motor powers and start the timer we created so we can periodically adjust the motor power to keep the robot going straight:

  // start motors turning forward at 20% power
  leftPower = 0.20;
  leftMotor.set(leftPower);
  // to go forward, left motor turns clockwise, right motor counter-clockwise
  rightPower = -0.20;
  rightMotor.set(rightPower);

  autoTimer.start();
  autoDone = false;

In the default section of autonomousPeriodic() periodically adjust the motor powers and stop if we've gone the desired distance.

     if (!autoDone) {
        // adjust motor power every 1/4 second to keep us going straight
        if (autoTimer.hasPeriodPassed(0.25)) {
          // if we're going straight, leftDistance == rightDistance
          // otherwise, compute the error and adjust the motor powers to minimize it
          double error = leftEncoder.getDistance() - rightEncoder.getDistance();
          double kP = 0.01; // proportion of error used to correct motor power
          // adjust motor power to try to keep left/right distances same
          leftPower  -= kP*error; // positive error means left is going too fast
          rightPower -= kP*error; // right motor spins opposite direction of left
          leftMotor.set(leftPower);
          rightMotor.set(rightPower);
        }
        if ((leftEncoder.getDistance() > 36.0) || 
            (rightEncoder.getDistance() > 36.0)) {
            leftMotor.stopMotor();
            rightMotor.stopMotor();
            autoDone = true;
        }
     }

This is the simplest form of closed-loop control: we are sensing position information (left, right distances) and adjusting powers proportionally (kP) to try to minimize the error. There are more sophisticated ways to do this that consider how quickly the robot is approaching its goal (derivative of error) and how long and how severely it has been off its target (integral of error). The PID control loop uses all of these for more precise and responsive closed loop control.

Notice that there are two critical values in this control loop:

How often the control loop runs (every 0.25 seconds in the above example)
How much the control loop adjusts the power based on the sensed inputs (kP=0.01 in the above example)
Try printing the error and adjusted motor powers using System.out.println(...) to see what adjustments are taking place.

Try experimenting with these values (carefully and changing them a little at a time) to see what effect they have. Be prepared to hit the Disable button or space bar when testing!

Study some examples

Learn more about robot programming by studying some simple example programs. Run them, study what they do, and incrementally expand their functionality. The WPILib software you installed includes example programs that you can compile and run on a RoboRIO. Review how to use VSCode and then try creating a new robot project.

WPILib provides 3 frameworks for building robot programs. Build your first program using the TimedRobot (previously had been IterativeRobot) framework as described here.

Connect your laptop to the RoboRIO USB port or WiFi
Connect your joystick or xbox controller to another USB port on your laptop
Launch the NI Driver Station Software. For details see here
Confirm that the software detects your joystick/controller and responds to button presses and stick movement
Use Microsoft Internet Explorer (not Chrome or Firefox) to connect to the RoboRIO's web interface (usually at http://172.22.11.2) and confirm that you have connectivity with the RobORIO
Create a test project and follow these instructions to build, load, and run it on your RoboRIO/Robot

Create another WPILib project based on the TimedRobot framework
- Study an example program. This is a simple program for one of our test robots: Peanut 2 (Hazelnut)
  - 2-wheel-drive (2WD) robot with two CIM motors driving plaction wheels
  - Motors are controlled by Talon SRX smart motor controllers; these are networked to the roboRIO (robot control computer) using CAN bus and are at addresses 3 (left motor) and 4 (right motor)
  - An ultrasonic sensor for forward collision avoidance connected to roboRIO digital I/O pins (DIO) 0 and 1
  - A USB camera so you can drive even when you can't see the robot
  - The driver controls the robot using the two analog sticks on an X-box game controller
- Copy the program below over the default Robot.java created by the framework
- Use the WPILib->Manage Vendor Libraries->Install New Libraries Offline->select the CTRE Framework

More sample code for Hazelnut (peanut robot) is here
A sample program to use ultrasonic sensors is here
A sample program to use xbox controllers is here
You can learn more about FRC robot programming here
You can learn more about game controllers here

Note: the sample program assumes two !TalonSRX motor controllers at IDs 3 and 4. You can set them using the CTRE Phoneix Tuner
Note: the !RoboRIO web web interface is available via browser at 10.25.37.2