Notes

package org.usfirst.frc.team5678.robot;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.opencv.core.Rect;
import org.opencv.imgproc.Imgproc;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.vision.VisionRunner;
import edu.wpi.first.wpilibj.vision.VisionThread;


/**
* This is a demo program showing the use of the RobotDrive class. The
* SampleRobot class is the base of a robot application that will automatically
* call your Autonomous and OperatorControl methods at the right time as
* controlled by the switches on the driver station or the field controls.
*
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the SampleRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*
* WARNING: While it may look like a good choice to use for your code if you're
* inexperienced, don't. Unless you know what you are doing, complex code will
* be much more difficult under this system. Use IterativeRobot or Command-Based
* instead if you're new.
*/
public class Robot extends SampleRobot {
private static final int IMG_WIDTH = 320;
private static final int IMG_HEIGHT = 240;
private VisionThread VisionThread;
private double centerX = 0.0;
private final Object imgLock = new Object();
RobotDrive myRobot = new RobotDrive(0, 1);
Joystick stick = new Joystick(0);
final String defaultAuto = "Default";
final String customAuto = "My Auto";
SendableChooser<String> chooser = new SendableChooser<>();

public Robot() {
myRobot.setExpiration(0.1);
}

@Override
public void robotInit() {
UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
camera.setResolution(IMG_WIDTH, IMG_HEIGHT);
VisionThread = new VisionThread(camera, new GripPipeline(), pipeline -> {
if (!pipeline.filterContoursOutput().empty()) {
Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput());
synchronized (imgLock) {
centerX = r.x + (r.width / 2);
}
}
VisionThread.start();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto modes", chooser);
}

/**
* This autonomous (along with the chooser code above) shows how to select
* between different autonomous modes using the dashboard. The sendable
* chooser code works with the Java SmartDashboard. If you prefer the
* LabVIEW Dashboard, remove all of the chooser code and uncomment the
* getString line to get the auto name from the text box below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the
* if-else structure below with additional strings. If using the
* SendableChooser make sure to add them to the chooser code above as well.
*/
@Override
public void autonomous() {
String autoSelected = chooser.getSelected();
// String autoSelected = SmartDashboard.getString("Auto Selector",
// defaultAuto);
System.out.println("Auto selected: " + autoSelected);

switch (autoSelected) {
case customAuto:
myRobot.setSafetyEnabled(false);
myRobot.drive(-0.5, 1.0); // spin at half speed
Timer.delay(2.0); // for 2 seconds
myRobot.drive(0.0, 0.0); // stop robot
break;
case defaultAuto:
default:
myRobot.setSafetyEnabled(false);
myRobot.drive(-0.5, 0.0); // drive forwards half speed
Timer.delay(2.0); // for 2 seconds
myRobot.drive(0.0, 0.0); // stop robot
break;
}
}

/**
* Runs the motors with arcade steering.
*/
@Override
public void operatorControl() {
myRobot.setSafetyEnabled(true);
while (isOperatorControl() && isEnabled()) {
myRobot.arcadeDrive(stick); // drive with arcade style (use right
// stick)
Timer.delay(0.005); // wait for a motor update time
}
}

/**
* Runs during test mode
*/
@Override
public void test() {
}
}