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

== First Program: Xbox Controller
We write robot software in VSCode just as we did for Java, but we'll make use of the WPILib extensions to create, build, and run our robot programs.  The WPILib extension is available via the W icon in the toolbar.  Click on the icon and then:
* Select Create a new project
* Select Project Type->Template->java->Iterative Robot
* Select a New Project Folder  and select (or create and then select) Documents\Robot Projects
* Enter project name: Xbox1
* Enter team number: 2537
* Press Generate Project 
* At the dialog select Yes (Current WIndow)
The WPILib extension then creates a new Robot project for you.  The project contains many files and they're listed and organized in the left tree.  The file we're intersted in is src->main->java->Robot.java  open that file by double-clicking on it.

The Robot.java class that was created for you contains the minimal set of functions required for a robot program operating in the WPIlib framework.  The functions include:
* robotInit() - called once when the robot starts
* robotPeriodic() - called periodically (e.g. every 20ms)
* autonomousInit() called one when the robot first enters autonomous mode
* autonomousPeriodic() - called periodically (e.g. every 20ms) while the robot is in autonomous mode
* teleopPeriodic() - called periodically while robot is in teleoperated mode
* testPeriodic() - called periodically while robot is in test mode

You will put your setup code in the Init() functions and the code that should run repeatedly in the Periodic() functions.  The code will make use of the pre-existing [https://first.wpi.edu/FRC/roborio/beta/docs/java/edu/wpi/first/wpilibj/XboxController.html XboxController] class provided in WPILib that makes it easy to use an Xbox controller.

Modify the newly created program as follows:
* Add to the imports
    {{{ 
          import edu.wpi.first.wpilibj.XboxController;
          import edu.wpi.first.wpilibj.GenericHID.Hand;
    }}}
* In the Robot class definition where variables are defined, add {{{   XboxController xbox; }}}
* In the function robotInit() add: {{{ xbox = new XboxController(0); }}}
* In the function robotPeriodic() add: {{{ SmartDashboard.putNumber("Left Joystick X", xbox.getX(Hand.kLeft)); }}}

Your program should look like [wiki://ControlSystems/SoftwareTeam/Training/IntroRobotJava/XboxExample this]

* Use the WPILib icon to build your new program: W->Build Robot Code
* Connect to the Peanut !WiFi Hotspot (the default password is password)
* Use the WPILib icon to deploy your program to the robot: W->Deploy Robot Code
* Launch the FRC Driver Station on your laptop.  Two windows should open: FRC Driver Station and FRC PC Dashboard.
* Plug in an x-box controller and make sure it is detected
* Make sure the Communications, Robot Code, and Joysticks LED icons are all green
* In the FRC PC Dashboard, select the Variables tab and scroll down to Smart Dashboard->Left Joystick X (that's the variable you created!)
* Move the left X-box joystick right and left and watch the value shown for Left Joystick X change!

You can even debug your program on the robot using the VSCode debugger just as you did with Java programs running on your laptop:
* Set a breakpoint on the line: {{{ xbox = new XboxController(0); }}}
* Use the WPILib icon to start debugging: W->Debug Robot Code
* The program will run until it gets to that line and then stop.  Press the continue button to continue running.
* Even though the program is running on the robot, you can set breakpoints, step through code, examine and change variables, etc. from your laptop.

* Extra credit: extend the program to print a message or display a value on the Smart Dashboard when a button is pressed on the xbox controller.

== Second program: Ultrasonic Rangefinder

Robots often need to sense the environment around them, particularly when driving autonomously.  Ultrasonic sensors are like the sonar used by bats.  They emit a chirp of sound and measure the time it takes to hear an echo of that chirp.  The longer the time, the further the sound traveled.  Since sound travels at a particular speed through air, we can use the echo delay to calculate how far away the object was that reflected the sound.  You can read more about ultrasonic sensors [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599715-ultrasonic-sensors-measuring-robot-distance-to-a-surface here]

Create another program using the !TimedRobot java template and name it !UltrasonicTest. Modify the generated code as follows:
* import the Ultrasonic class {{{ import edu.wpi.first.wpilibj.Ultrasonic; }}}
* declare a ultrasonic variable in the Robot class {{{ private Ultrasonic f_ultrasonic; }}}
* in robotInit() instantiate an ultrasonic object and set it to start automatically ranging 
  {{{
    f_ultrasonic = new Ultrasonic(1,0);
    f_ultrasonic.setAutomaticMode(true);
  }}}
* in robotPeriodic() read and display the range 
  {{{
    if (f_ultrasonic.isRangeValid()) {
       SmartDashboard.putNumber("Front range", f_ultrasonic.getRangeInches());
    }
  }}}

Run the program and observe the "Front range" value in the [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599724-displaying-data-on-the-ds-dashboard-overview Smart Dashboard] as you move the robot towards and away from the wall.

Notice that the measurements aren't always perfect; the sensor may receive echos from multiple surfaces.

== Third program: Servo Motor Control

Robots must be able to interact with the environment around them too.  Many types of actuators are used in FRC robotics; one of them is the [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599703-repeatable-low-power-movement-controlling-servos-with-wpilib Servo Motor].  A servo motor is a special type of motor that can rotate to a precise position, usually between 0 and 180 degrees.  They come in a variety of sizes and strengths.  You can connect a servo motor to any of the [http://www.ni.com/pdf/manuals/375274a.pdf#_OPENTOPIC_TOC_PROCESSING_d443e2165 PWM ports] on the !RoboRIO.  Examine the PWM ports and identify which row of pins are ground ([[Image(GroundSymbol.png,25px)]], +6V, and Signal (S).  Make sure you connect them to the proper pins on the Servo motor: black or brown goes to Ground, red or orange goes to +6V, yellow/white/blue goes to Signal.  You can read more about servo motors [https://learn.sparkfun.com/tutorials/servo-trigger-hookup-guide/all here]

Create another program using the !TimedRobot java template and name it !ServoTest. Modify the generated code as follows:
* Add the following imports:
{{{
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.Servo;
}}}
* declare variable for the Xbox controller and [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599703-repeatable-low-power-movement-controlling-servos-with-wpilib servo motor] in the Robot class 
{{{ 
   private XboxController xbox; 
   private Servo servo;
}}}
* in robotInit() instantiate the controller and motor objects
  {{{
    xbox = new XboxController(0); // Xbox controller on port 0
    servo = new Servo(1);         // Servo connected to PWM 1
  }}}
* in teleopPeriodic() read the xbox controller and adjust the servo motor accordingly
  {{{
    // Read xbox controller left joystick x axis
    // value returned is between -1.0 and 1.0
    double x = xbox.getX(Hand.kLeft); 
    // Servo motors are controlled on a scale of 0.0 to 1.0
    // so re-scale the X value to that range (-1.0->0, 0->0.5, 1.0->1.0)
    x = (x + 1.0) / 2;
    // Set servo motor position based on joystick
    servo.set(x); 
  }}}

Connect a Servo motor to PWM port 1 on the roboRIO and run your program.  You'll need to press the Enable button on the driver station to put the robot in Teleop mode so that teleopPeriodic() is called repeatedly.

* Extra credit: use buttons on the Xbox controller to set the servo to a specific position (e.g. 45-degrees, 90-degrees, 135-degrees).

== Fourth program: DC Motor Control
For stronger and faster movement, FRC robots use all sorts of motors.  There are large motors that propel the robot, small motors that drive flywheel or manipulators, etc.  Most DC motors use more power than can be supplied directly from the !RoboRIO so instead the roboRIO communicates with a dedicated high-power motor controller and tells it how fast and in which direction to move the motor.  FRC robots support several types of motor controllers; a common and powerful controller is the [https://www.andymark.com/products/talon-srx-speed-controller Talon SRX] often called a CAN Talon.

Create another program using the !TimedRobot java template and name it !MotorTest.
 
The motor controller Class is provided by a third party (Cross The Road Electronics aka CTRE); the team laptops already have the CTRE framework installed; if your laptop does not, you can download and install it [http://www.ctr-electronics.com/control-system/hro.html#product_tabs_technical_resources here].  To use the library, you need to add it to the project:
* WPILib->Manage Vendor Libraries
* -> Install new library (offline)
* -> Select ctre phoneix library  (or similar)

Modify the generated code as follows:

* Add the following imports:
{{{
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import com.ctre.phoenix.motorcontrol.can.*;
}}}
* declare variable for the Xbox controller and [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599703-repeatable-low-power-movement-controlling-servos-with-wpilib servo motor] in the Robot class 
{{{ 
   private XboxController xbox; 
   private WPI_TalonSRX m_rearLeft;
}}}
* in robotInit() instantiate the controller and motor controller objects
  {{{
    xbox = new XboxController(0);     // Xbox controller on port 0
    m_rearLeft = new WPI_TalonSRX(4); // CAN Talon ID 4
  }}}
* in teleopPeriodic() read the xbox controller and adjust the DC motor accordingly
  {{{
     // Read xbox controller left joystick x axis
     // value returned is between -1.0 and 1.0
     double x = xbox.getX(Hand.kLeft); 
     // DC Motor controllers apply between -1.0 (full reverse)
     // 0=stop and +1.0 (full forward) power
     // Set motor speed based on joystick
     m_rearLeft.set(x);
  }}}

Note: you'll need to press the Enable button on the driver station to put the robot in Teleop mode so that teleopPeriodic() is called repeatedly.

* Extra Credit: use the left joystick Y axis to control the left motor power and the right joystick Y axis to control the right motor power...now you have a driveable robot!

== 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 [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599717-encoders-measuring-rotation-of-a-wheel-or-other-shaft 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 Control!
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);

    // to go forward, left motor turns clockwise, right motor counter-clockwise
    rightMotor.setInverted(true);
  }}}

* 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);
    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
  {{{
      default:
        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.


== 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 [https://wpilib.screenstepslive.com/s/currentCS/m/79833/l/941601-vs-code-basics-and-wpilib-in-vs-code how to use VSCode] and then try [https://wpilib.screenstepslive.com/s/currentCS/m/79833/l/932465-creating-a-new-wpilib-project-in-vs-code creating a new robot project].

* WPILib provides [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599697-choosing-a-base-class 3 frameworks] for building robot programs.  Build your first program using the !TimedRobot (previously had been !IterativeRobot) framework as described [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/145307-creating-your-benchtop-test-program 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 [https://wpilib.screenstepslive.com/s/4485/m/24192/l/144976-frc-driver-station-powered-by-ni-labview 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 [https://wpilib.screenstepslive.com/s/currentCS/m/79833/l/932465-creating-a-new-wpilib-project-in-vs-code these instructions] to build, load, and run it on your RoboRIO/Robot

* Create another WPILib project based on the !TimedRobot framework 
   * Study an [wiki://ControlSystems/IntroRobotJava/HazelnutSample1 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 [wiki:ControlSystems/SampleCode/Hazelnut here]
* A sample program to use ultrasonic sensors is [wiki:ControlSystems/SampleCode/Ultrasonic here]
* A sample program to use xbox controllers is [wiki:ControlSystems/SampleCode/XboxController here]
* You can learn more about FRC robot programming [https://frc-pdr.readthedocs.io/en/latest/index.html here]
* You can learn more about game controllers [https://github.com/MTHSRoboticsClub/Documentation/wiki/Drive-Controller-Comparison 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