diff --git a/build.gradle b/build.gradle index 330e17cd90..f5c785c99b 100644 --- a/build.gradle +++ b/build.gradle @@ -174,6 +174,10 @@ subprojects { apply plugin: 'idea' apply plugin: 'checkstyle' + repositories { + mavenCentral() + } + checkstyle { toolVersion = "8.1" configFile = new File(rootDir, "styleguide/checkstyle.xml") diff --git a/settings.gradle b/settings.gradle index 47b3b64157..1c2722a78b 100644 --- a/settings.gradle +++ b/settings.gradle @@ -3,7 +3,8 @@ include 'ni-libraries' include 'hal' include 'wpilibc' include 'wpilibcIntegrationTests' -include 'wpilibj' include 'wpilibc-examples' +include 'wpilibj' include 'wpilibjIntegrationTests' +include 'wpilibjExamples' include 'myRobot' diff --git a/styleguide/checkstyleEclipse.xml b/styleguide/checkstyleEclipse.xml new file mode 100644 index 0000000000..ee6ca99e33 --- /dev/null +++ b/styleguide/checkstyleEclipse.xml @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle new file mode 100644 index 0000000000..7fdb90b9c5 --- /dev/null +++ b/wpilibjExamples/build.gradle @@ -0,0 +1,28 @@ +apply plugin: 'java' +apply plugin: 'pmd' + +dependencies { + compile project(':wpilibj') + + compile 'edu.wpi.first.wpiutil:wpiutil-java:3.+' + compile 'edu.wpi.first.ntcore:ntcore-java:4.+' + compile 'org.opencv:opencv-java:3.2.0' + compile 'edu.wpi.first.cscore:cscore-java:1.+' +} + +checkstyle { + configFile = new File(rootDir, "styleguide/checkstyleEclipse.xml") +} + +pmd { + consoleOutput = true + reportsDir = file("$project.buildDir/reports/pmd") + ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml")) + ruleSets = [] +} + +gradle.projectsEvaluated { + tasks.withType(JavaCompile) { + options.compilerArgs << "-Xlint:unchecked" << "-Xlint:deprecation" << "-Werror" + } +} diff --git a/wpilibjExamples/examples.xml b/wpilibjExamples/examples.xml new file mode 100755 index 0000000000..213286b807 --- /dev/null +++ b/wpilibjExamples/examples.xml @@ -0,0 +1,301 @@ + + + + + + Getting Started with Java + Examples for getting started with FRC Java + + + Actuators + Example programs that demonstrate the use of various actuators + + + Analog + Examples programs that show different uses of analog inputs, + outputs and various analog sensors + + + CAN + Example programs that demonstrate the use of the CAN components in the control + system + + + Digital + Example programs that demonstrate the sensors that use the digital I/O ports + + + I2C + Example programs that demonstrate the use of I2C and various sensors that use + it + + + Joystick + Example programs that demonstate different uses of joysticks for robot + driving + + + NetworkTables + Examples of how to use NetworkTables to accomplish a + variety of tasks such as sending and receiving values to both + dashboards and co-processors. + + + Pneumatics + Example programs that demonstrate the use of the compressor and solenoids + + + Robot and Motor + Example programs that demonstrate driving a robot and motors including safety, + servos, etc. + + + Safety + Example programs that demonstate the motor safety classes and how to use them + with your programs + + + Sensors + Example programs that demonstrate the use of the various commonly used sensors + on FRC robots + + + SPI + Example programs that demonstrate the use of the SPI bus and sensors that + connect to it + + + Vision + Example programs that demonstrate the use of cameras and image processing + + + Complete Robot + Complete Robot example programs + + + + + Getting Started + An example program which demonstrates the simplest autonomous and + teleoperated routines. + + Getting Started with Java + + + src/$package-dir + + + + + + + Tank Drive + Demonstrate the use of the RobotDrive class doing teleop driving with tank + steering + + Actuators + Joystick + Robot and Motor + Safety + + + src/$package-dir + + + + + + + Mecanum Drive + Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum + steering + + Actuators + Joystick + Robot and Motor + Safety + + + src/$package-dir + + + + + + + Ultrasonic + Demonstrate maintaining a set distance using an ultrasonic sensor. + + Sensors + Robot and Motor + Analog + + + src/$package-dir + + + + + + + Ultrasonic PID + Demonstrate maintaining a set distance using an ultrasonic sensor and PID + Control. + + Sensors + Robot and Motor + Analog + + + src/$package-dir + + + + + + + Potentiometer PID + An example to demonstrate the use of a potentiometer and PID control to reach + elevator position setpoints. + + Sensors + Actuators + Analog + Joystick + + + src/$package-dir + + + + + + + Gyro + An example program showing how to drive straight with using a gyro sensor. + + Sensors + Robot and Motor + Analog + Joystick + + + src/$package-dir + + + + + + + Gyro Mecanum + An example program showing how to perform mecanum drive with field oriented + controls. + + Sensors + Robot and Motor + Analog + Joystick + + + src/$package-dir + + + + + + + Motor Controller + Demonstrate controlling a single motor with a joystick + + Actuators + Joystick + Robot and Motor + + + src/$package-dir + + + + + + + GearsBot + A fully functional example CommandBased program for WPIs GearsBot robot. This + code can run on your computer if it supports simulation. + + Complete Robot + + /usr/share/frcsim/worlds/GearsBotDemo.world + + src/$package-dir + src/$package-dir/commands + src/$package-dir/subsystems + + + + + + + + + + + + + + + + + + + + + + + Simple Vision + Demonstrate the use of the CameraServer class to stream from a USB Webcam + without processing the images. + + Vision + Complete List + + + src/$package-dir + + + + + + + Intermediate Vision + Demonstrate the use of the NIVision class to capture image from a Webcam, + process them, and then send them to the dashboard. + + Vision + Complete List + + + src/$package-dir + + + + + + + Axis Camera Sample + An example program that acquires images from an Axis network camera and adds + some + annotation to the image as you might do for showing operators the result of some image + recognition, and sends it to the dashboard for display. This demonstrates the use of the + AxisCamera class. + + Vision + + + src/$package-dir + + + + + + diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java new file mode 100644 index 0000000000..3c197022f8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.JoystickButton; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This class is the glue that binds the controls on the physical operator + * interface to the commands and command groups that allow control of the robot. + */ +public class OI { + + private Joystick m_joystick = new Joystick(0); + + public OI() { + // Put Some buttons on the SmartDashboard + SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0)); + SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2)); + SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3)); + + SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0)); + SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45)); + + SmartDashboard.putData("Open Claw", new OpenClaw()); + SmartDashboard.putData("Close Claw", new CloseClaw()); + + SmartDashboard.putData("Deliver Soda", new Autonomous()); + + // Create some buttons + JoystickButton dpadUp = new JoystickButton(m_joystick, 5); + JoystickButton dpadRight = new JoystickButton(m_joystick, 6); + JoystickButton dpadDown = new JoystickButton(m_joystick, 7); + JoystickButton dpadLeft = new JoystickButton(m_joystick, 8); + JoystickButton l2 = new JoystickButton(m_joystick, 9); + JoystickButton r2 = new JoystickButton(m_joystick, 10); + JoystickButton l1 = new JoystickButton(m_joystick, 11); + JoystickButton r1 = new JoystickButton(m_joystick, 12); + + // Connect the buttons to commands + dpadUp.whenPressed(new SetElevatorSetpoint(0.2)); + dpadDown.whenPressed(new SetElevatorSetpoint(-0.2)); + dpadRight.whenPressed(new CloseClaw()); + dpadLeft.whenPressed(new OpenClaw()); + + r1.whenPressed(new PrepareToPickup()); + r2.whenPressed(new Pickup()); + l1.whenPressed(new Place()); + l2.whenPressed(new Autonomous()); + } + + public Joystick getJoystick() { + return m_joystick; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java new file mode 100644 index 0000000000..e74fc1e0a3 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous; +import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw; +import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain; +import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator; +import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the IterativeRobot + * 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. + */ +public class Robot extends IterativeRobot { + + Command m_autonomousCommand; + + public static DriveTrain m_drivetrain; + public static Elevator m_elevator; + public static Wrist m_wrist; + public static Claw m_claw; + public static OI m_oi; + + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + // Initialize all subsystems + m_drivetrain = new DriveTrain(); + m_elevator = new Elevator(); + m_wrist = new Wrist(); + m_claw = new Claw(); + m_oi = new OI(); + + // instantiate the command used for the autonomous period + m_autonomousCommand = new Autonomous(); + + // Show what command your subsystem is running on the SmartDashboard + SmartDashboard.putData(m_drivetrain); + SmartDashboard.putData(m_elevator); + SmartDashboard.putData(m_wrist); + SmartDashboard.putData(m_claw); + } + + @Override + public void autonomousInit() { + m_autonomousCommand.start(); // schedule the autonomous command (example) + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + log(); + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + m_autonomousCommand.cancel(); + } + + /** + * This function is called periodically during teleoperated mode. + */ + @Override + public void teleopPeriodic() { + Scheduler.getInstance().run(); + log(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + LiveWindow.run(); + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + private void log() { + m_wrist.log(); + m_elevator.log(); + m_drivetrain.log(); + m_claw.log(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java new file mode 100644 index 0000000000..0b01169091 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * The main autonomous command to pickup and deliver the soda to the box. + */ +public class Autonomous extends CommandGroup { + public Autonomous() { + addSequential(new PrepareToPickup()); + addSequential(new Pickup()); + addSequential(new SetDistanceToBox(0.10)); + // addSequential(new DriveStraight(4)); // Use Encoders if ultrasonic is + // broken + addSequential(new Place()); + addSequential(new SetDistanceToBox(0.60)); + // addSequential(new DriveStraight(-2)); // Use Encoders if ultrasonic + // is broken + addParallel(new SetWristSetpoint(-45)); + addSequential(new CloseClaw()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java new file mode 100644 index 0000000000..64197dde69 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Closes the claw for one second. Real robots should use sensors, stalling + * motors is BAD! + */ +public class CloseClaw extends Command { + public CloseClaw() { + requires(Robot.m_claw); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_claw.close(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Robot.m_claw.isGrabbing(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + // NOTE: Doesn't stop in simulation due to lower friction causing the + // can to fall out + // + there is no need to worry about stalling the motor or crushing the + // can. + if (!Robot.isSimulation()) { + Robot.m_claw.stop(); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java new file mode 100644 index 0000000000..12ad2e853f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Drive the given distance straight (negative values go backwards). Uses a + * local PID controller to run a simple PID loop that is only enabled while this + * command is running. The input is the averaged values of the left and right + * encoders. + */ +public class DriveStraight extends Command { + private PIDController m_pid; + + public DriveStraight(double distance) { + requires(Robot.m_drivetrain); + m_pid = new PIDController(4, 0, 0, new PIDSource() { + PIDSourceType m_sourceType = PIDSourceType.kDisplacement; + + @Override + public double pidGet() { + return Robot.m_drivetrain.getDistance(); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_sourceType = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_sourceType; + } + }, d -> Robot.m_drivetrain.drive(d, d)); + + m_pid.setAbsoluteTolerance(0.01); + m_pid.setSetpoint(distance); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // Get everything in a safe starting state. + Robot.m_drivetrain.reset(); + m_pid.reset(); + m_pid.enable(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return m_pid.onTarget(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Stop PID and the wheels + m_pid.disable(); + Robot.m_drivetrain.drive(0, 0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java new file mode 100644 index 0000000000..9d2328a85d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.TimedCommand; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Opens the claw for one second. Real robots should use sensors, stalling + * motors is BAD! + */ +public class OpenClaw extends TimedCommand { + public OpenClaw() { + super(1); + requires(Robot.m_claw); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_claw.open(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.m_claw.stop(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java new file mode 100644 index 0000000000..523820ad23 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java @@ -0,0 +1,22 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * Pickup a soda can (if one is between the open claws) and get it in a safe + * state to drive around. + */ +public class Pickup extends CommandGroup { + public Pickup() { + addSequential(new CloseClaw()); + addParallel(new SetWristSetpoint(-45)); + addSequential(new SetElevatorSetpoint(0.25)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java new file mode 100644 index 0000000000..f38c6bf18f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * Place a held soda can onto the platform. + */ +public class Place extends CommandGroup { + public Place() { + addSequential(new SetElevatorSetpoint(0.25)); + addSequential(new SetWristSetpoint(0)); + addSequential(new OpenClaw()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java new file mode 100644 index 0000000000..6714a4e5c5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * Make sure the robot is in a state to pickup soda cans. + */ +public class PrepareToPickup extends CommandGroup { + public PrepareToPickup() { + addParallel(new OpenClaw()); + addParallel(new SetWristSetpoint(0)); + addSequential(new SetElevatorSetpoint(0)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java new file mode 100644 index 0000000000..b96aa67f83 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Drive until the robot is the given distance away from the box. Uses a local + * PID controller to run a simple PID loop that is only enabled while this + * command is running. The input is the averaged values of the left and right + * encoders. + */ +public class SetDistanceToBox extends Command { + + private PIDController m_pid; + + public SetDistanceToBox(double distance) { + requires(Robot.m_drivetrain); + m_pid = new PIDController(-2, 0, 0, new PIDSource() { + PIDSourceType m_sourceType = PIDSourceType.kDisplacement; + + @Override + public double pidGet() { + return Robot.m_drivetrain.getDistanceToObstacle(); + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + m_sourceType = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return m_sourceType; + } + }, d -> Robot.m_drivetrain.drive(d, d)); + + m_pid.setAbsoluteTolerance(0.01); + m_pid.setSetpoint(distance); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // Get everything in a safe starting state. + Robot.m_drivetrain.reset(); + m_pid.reset(); + m_pid.enable(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return m_pid.onTarget(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Stop PID and the wheels + m_pid.disable(); + Robot.m_drivetrain.drive(0, 0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java new file mode 100644 index 0000000000..2e4f727e96 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Move the elevator to a given location. This command finishes when it is + * within the tolerance, but leaves the PID loop running to maintain the + * position. Other commands using the elevator should make sure they disable + * PID! + */ +public class SetElevatorSetpoint extends Command { + private double m_setpoint; + + public SetElevatorSetpoint(double setpoint) { + m_setpoint = setpoint; + requires(Robot.m_elevator); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_elevator.enable(); + Robot.m_elevator.setSetpoint(m_setpoint); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Robot.m_elevator.onTarget(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java new file mode 100644 index 0000000000..1b5f941880 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Move the wrist to a given angle. This command finishes when it is within the + * tolerance, but leaves the PID loop running to maintain the position. Other + * commands using the wrist should make sure they disable PID! + */ +public class SetWristSetpoint extends Command { + private double m_setpoint; + + public SetWristSetpoint(double setpoint) { + m_setpoint = setpoint; + requires(Robot.m_wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_wrist.enable(); + Robot.m_wrist.setSetpoint(m_setpoint); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Robot.m_wrist.onTarget(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java new file mode 100644 index 0000000000..d4e0ee895e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Have the robot drive tank style using the PS3 Joystick until interrupted. + */ +public class TankDriveWithJoystick extends Command { + + public TankDriveWithJoystick() { + requires(Robot.m_drivetrain); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.m_drivetrain.drive(Robot.m_oi.getJoystick()); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; // Runs until interrupted + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.m_drivetrain.drive(0, 0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java new file mode 100644 index 0000000000..4aa72f4791 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Victor; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + +/** + * The claw subsystem is a simple system with a motor for opening and closing. + * If using stronger motors, you should probably use a sensor so that the motors + * don't stall. + */ +public class Claw extends Subsystem { + + private SpeedController m_motor = new Victor(7); + private DigitalInput m_contact = new DigitalInput(5); + + public Claw() { + super(); + + // Let's show everything on the LiveWindow + LiveWindow.addActuator("Claw", "Motor", (Victor) m_motor); + LiveWindow.addActuator("Claw", "Limit Switch", m_contact); + } + + @Override + public void initDefaultCommand() { + } + + public void log() { + } + + /** + * Set the claw motor to move in the open direction. + */ + public void open() { + m_motor.set(-1); + } + + /** + * Set the claw motor to move in the close direction. + */ + public void close() { + m_motor.set(1); + } + + /** + * Stops the claw motor from moving. + */ + public void stop() { + m_motor.set(0); + } + + /** + * Return true when the robot is grabbing an object hard enough to trigger + * the limit switch. + */ + public boolean isGrabbing() { + return m_contact.get(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java new file mode 100644 index 0000000000..db0ba4b1ce --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java @@ -0,0 +1,146 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Joystick.AxisType; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; +import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDriveWithJoystick; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * The DriveTrain subsystem incorporates the sensors and actuators attached to + * the robots chassis. These include four drive motors, a left and right encoder + * and a gyro. + */ +public class DriveTrain extends Subsystem { + + private SpeedController m_leftMotor + = new SpeedControllerGroup(new Spark(0), new Spark(1)); + private SpeedController m_rightMotor + = new SpeedControllerGroup(new Spark(2), new Spark(3)); + + private DifferentialDrive m_drive + = new DifferentialDrive(m_leftMotor, m_rightMotor); + + private Encoder m_leftEncoder = new Encoder(1, 2); + private Encoder m_rightEncoder = new Encoder(3, 4); + private AnalogInput m_rangefinder = new AnalogInput(6); + private AnalogGyro m_gyro = new AnalogGyro(1); + + public DriveTrain() { + super(); + + // Encoders may measure differently in the real world and in + // simulation. In this example the robot moves 0.042 barleycorns + // per tick in the real world, but the simulated encoders + // simulate 360 tick encoders. This if statement allows for the + // real robot to handle this difference in devices. + if (Robot.isReal()) { + m_leftEncoder.setDistancePerPulse(0.042); + m_rightEncoder.setDistancePerPulse(0.042); + } else { + // Circumference in ft = 4in/12(in/ft)*PI + m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); + m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); + } + + // Let's show the sensors on the LiveWindow + LiveWindow.addSensor("Drive Train", "Left Encoder", m_leftEncoder); + LiveWindow.addSensor("Drive Train", "Right Encoder", m_rightEncoder); + LiveWindow.addSensor("Drive Train", "Rangefinder", m_rangefinder); + LiveWindow.addSensor("Drive Train", "Gyro", m_gyro); + } + + /** + * When no other command is running let the operator drive around using the + * PS3 joystick. + */ + @Override + public void initDefaultCommand() { + setDefaultCommand(new TankDriveWithJoystick()); + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + public void log() { + SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance()); + SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance()); + SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate()); + SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate()); + SmartDashboard.putNumber("Gyro", m_gyro.getAngle()); + } + + /** + * Tank style driving for the DriveTrain. + * + * @param left + * Speed in range [-1,1] + * @param right + * Speed in range [-1,1] + */ + public void drive(double left, double right) { + m_drive.tankDrive(left, right); + } + + /** + * Tank style driving for the DriveTrain. + * + * @param joy The ps3 style joystick to use to drive tank style. + */ + public void drive(Joystick joy) { + drive(-joy.getY(), -joy.getAxis(AxisType.kThrottle)); + } + + /** + * Get the robot's heading. + * + * @return The robots heading in degrees. + */ + public double getHeading() { + return m_gyro.getAngle(); + } + + /** + * Reset the robots sensors to the zero states. + */ + public void reset() { + m_gyro.reset(); + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Get the average distance of the encoders since the last reset. + * + * @return The distance driven (average of left and right encoders). + */ + public double getDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2; + } + + /** + * Get the distance to the obstacle. + * + * @return The distance to the obstacle detected by the rangefinder. + */ + public double getDistanceToObstacle() { + // Really meters in simulation since it's a rangefinder... + return m_rangefinder.getAverageVoltage(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java new file mode 100644 index 0000000000..79d0cd51e3 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java @@ -0,0 +1,85 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Victor; +import edu.wpi.first.wpilibj.command.PIDSubsystem; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; +import edu.wpi.first.wpilibj.interfaces.Potentiometer; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * The elevator subsystem uses PID to go to a given height. Unfortunately, in + * it's current state PID values for simulation are different than in the real + * world do to minor differences. + */ +public class Elevator extends PIDSubsystem { + + private SpeedController m_motor; + private Potentiometer m_pot; + + private static final double kP_real = 4; + private static final double kI_real = 0.07; + private static final double kP_simulation = 18; + private static final double kI_simulation = 0.2; + + public Elevator() { + super(kP_real, kI_real, 0); + if (Robot.isSimulation()) { // Check for simulation and update PID values + getPIDController().setPID(kP_simulation, kI_simulation, 0, 0); + } + setAbsoluteTolerance(0.005); + + m_motor = new Victor(5); + + // Conversion value of potentiometer varies between the real world and + // simulation + if (Robot.isReal()) { + m_pot = new AnalogPotentiometer(2, -2.0 / 5); + } else { + m_pot = new AnalogPotentiometer(2); // Defaults to meters + } + + // Let's show everything on the LiveWindow + LiveWindow.addActuator("Elevator", "Motor", (Victor) m_motor); + LiveWindow.addSensor("Elevator", "Pot", (AnalogPotentiometer) m_pot); + LiveWindow.addActuator("Elevator", "PID", getPIDController()); + } + + @Override + public void initDefaultCommand() { + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + public void log() { + SmartDashboard.putData("Wrist Pot", (AnalogPotentiometer) m_pot); + } + + /** + * Use the potentiometer as the PID sensor. This method is automatically + * called by the subsystem. + */ + @Override + protected double returnPIDInput() { + return m_pot.get(); + } + + /** + * Use the motor as the PID output. This method is automatically called by + * the subsystem. + */ + @Override + protected void usePIDOutput(double power) { + m_motor.set(power); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java new file mode 100644 index 0000000000..9414f9ecbd --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java @@ -0,0 +1,82 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Victor; +import edu.wpi.first.wpilibj.command.PIDSubsystem; +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; +import edu.wpi.first.wpilibj.interfaces.Potentiometer; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * The wrist subsystem is like the elevator, but with a rotational joint instead + * of a linear joint. + */ +public class Wrist extends PIDSubsystem { + + private SpeedController m_motor; + private Potentiometer m_pot; + + private static final double kP_real = 1; + private static final double kP_simulation = 0.05; + + public Wrist() { + super(kP_real, 0, 0); + if (Robot.isSimulation()) { // Check for simulation and update PID values + getPIDController().setPID(kP_simulation, 0, 0, 0); + } + setAbsoluteTolerance(2.5); + + m_motor = new Victor(6); + + // Conversion value of potentiometer varies between the real world and + // simulation + if (Robot.isReal()) { + m_pot = new AnalogPotentiometer(3, -270.0 / 5); + } else { + m_pot = new AnalogPotentiometer(3); // Defaults to degrees + } + + // Let's show everything on the LiveWindow + LiveWindow.addActuator("Wrist", "Motor", (Victor) m_motor); + LiveWindow.addSensor("Wrist", "Pot", (AnalogPotentiometer) m_pot); + LiveWindow.addActuator("Wrist", "PID", getPIDController()); + } + + @Override + public void initDefaultCommand() { + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + public void log() { + SmartDashboard.putData("Wrist Angle", (AnalogPotentiometer) m_pot); + } + + /** + * Use the potentiometer as the PID sensor. This method is automatically + * called by the subsystem. + */ + @Override + protected double returnPIDInput() { + return m_pot.get(); + } + + /** + * Use the motor as the PID output. This method is automatically called by + * the subsystem. + */ + @Override + protected void usePIDOutput(double power) { + m_motor.set(power); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java new file mode 100644 index 0000000000..f2b2e5f1e5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gettingstarted; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the IterativeRobot + * 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. + */ +public class Robot extends IterativeRobot { + + private DifferentialDrive m_robotDrive + = new DifferentialDrive(new Spark(0), new Spark(1)); + private Joystick m_stick = new Joystick(0); + private Timer m_timer = new Timer(); + + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + } + + /** + * This function is run once each time the robot enters autonomous mode. + */ + @Override + public void autonomousInit() { + m_timer.reset(); + m_timer.start(); + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + // Drive for 2 seconds + if (m_timer.get() < 2.0) { + m_robotDrive.arcadeDrive(0.5, 0.0); // drive forwards half speed + } else { + m_robotDrive.stopMotor(); // stop robot + } + } + + /** + * This function is called once each time the robot enters teleoperated mode. + */ + @Override + public void teleopInit() { + } + + /** + * This function is called periodically during teleoperated mode. + */ + @Override + public void teleopPeriodic() { + m_robotDrive.arcadeDrive(m_stick.getY(), m_stick.getX()); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + LiveWindow.run(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java new file mode 100644 index 0000000000..4cc98911a5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyro; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; + +/** + * This is a sample program to demonstrate how to use a gyro sensor to make a + * robot drive straight. This program uses a joystick to drive forwards and + * backwards while the gyro is used for direction keeping. + */ +public class Robot extends IterativeRobot { + + private static final double kAngleSetpoint = 0.0; + private static final double kP = 0.005; // propotional turning constant + + // gyro calibration constant, may need to be adjusted; + // gyro value of 360 is set to correspond to one full revolution + private static final double kVoltsPerDegreePerSecond = 0.0128; + + private static final int kLeftMotorPort = 0; + private static final int kRightMotorPort = 1; + private static final int kGyroPort = 0; + private static final int kJoystickPort = 0; + + private DifferentialDrive m_myRobot + = new DifferentialDrive(new Spark(kLeftMotorPort), + new Spark(kRightMotorPort)); + private AnalogGyro m_gyro = new AnalogGyro(kGyroPort); + private Joystick m_joystick = new Joystick(kJoystickPort); + + @Override + public void robotInit() { + m_gyro.setSensitivity(kVoltsPerDegreePerSecond); + } + + /** + * The motor speed is set from the joystick while the RobotDrive turning + * value is assigned from the error between the setpoint and the gyro angle. + */ + @Override + public void teleopPeriodic() { + double turningValue = (kAngleSetpoint - m_gyro.getAngle()) * kP; + // Invert the direction of the turn if we are going backwards + turningValue = Math.copySign(turningValue, m_joystick.getY()); + m_myRobot.arcadeDrive(m_joystick.getY(), turningValue); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java new file mode 100644 index 0000000000..d540712b9d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyromecanum; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.drive.MecanumDrive; + +/** + * This is a sample program that uses mecanum drive with a gyro sensor to + * maintian rotation vectorsin relation to the starting orientation of the robot + * (field-oriented controls). + */ +public class Robot extends IterativeRobot { + // gyro calibration constant, may need to be adjusted; + // gyro value of 360 is set to correspond to one full revolution + private static final double kVoltsPerDegreePerSecond = 0.0128; + + private static final int kFrontLeftChannel = 0; + private static final int kRearLeftChannel = 1; + private static final int kFrontRightChannel = 2; + private static final int kRearRightChannel = 3; + private static final int kGyroPort = 0; + private static final int kJoystickPort = 0; + + private MecanumDrive m_robotDrive; + private AnalogGyro m_gyro = new AnalogGyro(kGyroPort); + private Joystick m_joystick = new Joystick(kJoystickPort); + + @Override + public void robotInit() { + Spark frontLeft = new Spark(kFrontLeftChannel); + Spark rearLeft = new Spark(kRearLeftChannel); + Spark frontRight = new Spark(kFrontRightChannel); + Spark rearRight = new Spark(kRearRightChannel); + + // Invert the left side motors. + // You may need to change or remove this to match your robot. + frontLeft.setInverted(true); + rearLeft.setInverted(true); + + m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight); + + m_gyro.setSensitivity(kVoltsPerDegreePerSecond); + } + + /** + * Mecanum drive is used with the gyro angle as an input. + */ + @Override + public void teleopPeriodic() { + m_robotDrive.driveCartesian(m_joystick.getX(), m_joystick.getY(), + m_joystick.getZ(), m_gyro.getAngle()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java new file mode 100755 index 0000000000..9e7813d653 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.mecanumdrive; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.drive.MecanumDrive; + +/** + * This is a demo program showing how to use Mecanum control with the RobotDrive + * class. + */ +public class Robot extends IterativeRobot { + + private static final int kFrontLeftChannel = 2; + private static final int kRearLeftChannel = 3; + private static final int kFrontRightChannel = 1; + private static final int kRearRightChannel = 0; + + private static final int kJoystickChannel = 0; + + private MecanumDrive m_robotDrive; + private Joystick m_stick; + + @Override + public void robotInit() { + Spark frontLeft = new Spark(kFrontLeftChannel); + Spark rearLeft = new Spark(kRearLeftChannel); + Spark frontRight = new Spark(kFrontRightChannel); + Spark rearRight = new Spark(kRearRightChannel); + + // Invert the left side motors. + // You may need to change or remove this to match your robot. + frontLeft.setInverted(true); + rearLeft.setInverted(true); + + m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight); + + m_stick = new Joystick(kJoystickChannel); + } + + @Override + public void teleopPeriodic() { + // Use the joystick X axis for lateral movement, Y axis for forward + // movement, and Z axis for rotation. + m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(), + m_stick.getZ(), 0.0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java new file mode 100755 index 0000000000..8b4844ef29 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.motorcontrol; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Spark; + +/** + * This sample program shows how to control a motor using a joystick. In the + * operator control part of the program, the joystick is read and the value is + * written to the motor. + * + *

Joystick analog values range from -1 to 1 and speed controller inputs also + * range from -1 to 1 making it easy to work together. + */ +public class Robot extends IterativeRobot { + + private static final int kMotorPort = 0; + private static final int kJoystickPort = 0; + + private SpeedController m_motor; + private Joystick m_joystick; + + @Override + public void robotInit() { + m_motor = new Spark(kMotorPort); + m_joystick = new Joystick(kJoystickPort); + } + + @Override + public void teleopPeriodic() { + m_motor.set(m_joystick.getY()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java new file mode 100644 index 0000000000..230091e811 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.potentiometerpid; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.Joystick; + +/** + * This is a sample program to demonstrate how to use a soft potentiometer and a + * PID controller to reach and maintain position setpoints on an elevator + * mechanism. + */ +public class Robot extends IterativeRobot { + + private static final int kPotChannel = 1; + private static final int kMotorChannel = 7; + private static final int kJoystickChannel = 0; + + // bottom, middle, and top elevator setpoints + private static final double[] kSetPoints = {1.0, 2.6, 4.3}; + + // proportional, integral, and derivative speed constants; motor inverted + // DANGER: when tuning PID constants, high/inappropriate values for kP, kI, + // and kD may cause dangerous, uncontrollable, or undesired behavior! + // these may need to be positive for a non-inverted motor + private static final double kP = -5.0; + private static final double kI = -0.02; + private static final double kD = -2.0; + + private PIDController m_pidController; + private AnalogInput m_potentiometer; + private SpeedController m_elevatorMotor; + private Joystick m_joystick; + + private int m_index = 0; + private boolean m_previousButtonValue = false; + + @Override + public void robotInit() { + m_potentiometer = new AnalogInput(kPotChannel); + m_elevatorMotor = new Spark(kMotorChannel); + m_joystick = new Joystick(kJoystickChannel); + + m_pidController + = new PIDController(kP, kI, kD, m_potentiometer, m_elevatorMotor); + m_pidController.setInputRange(0, 5); + } + + @Override + public void teleopInit() { + m_pidController.enable(); + } + + @Override + public void teleopPeriodic() { + // when the button is pressed once, the selected elevator setpoint + // is incremented + boolean currentButtonValue = m_joystick.getTrigger(); + if (currentButtonValue && !m_previousButtonValue) { + // index of the elevator setpoint wraps around. + m_index = (m_index + 1) % kSetPoints.length; + } + m_previousButtonValue = currentButtonValue; + + m_pidController.setSetpoint(kSetPoints[m_index]); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java new file mode 100755 index 0000000000..5ea527cdb0 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.tankdrive; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; + +/** + * This is a demo program showing the use of the RobotDrive class, specifically + * it contains the code necessary to operate a robot with tank drive. + */ +public class Robot extends IterativeRobot { + + private DifferentialDrive m_myRobot; + private Joystick m_leftStick; + private Joystick m_rightStick; + + @Override + public void robotInit() { + m_myRobot = new DifferentialDrive(new Spark(0), new Spark(1)); + m_leftStick = new Joystick(0); + m_rightStick = new Joystick(1); + } + + @Override + public void teleopPeriodic() { + m_myRobot.tankDrive(m_leftStick.getY(), m_rightStick.getY()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java new file mode 100644 index 0000000000..203ff363be --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.ultrasonic; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; + +/** + * This is a sample program demonstrating how to use an ultrasonic sensor and + * proportional control to maintain a set distance from an object. + */ + +public class Robot extends IterativeRobot { + // distance in inches the robot wants to stay from an object + private static final double kHoldDistance = 12.0; + + // factor to convert sensor values to a distance in inches + private static final double kValueToInches = 0.125; + + // proportional speed constant + private static final double kP = 0.05; + + private static final int kLeftMotorPort = 0; + private static final int kRightMotorPort = 1; + private static final int kUltrasonicPort = 0; + + private AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort); + private DifferentialDrive m_robotDrive + = new DifferentialDrive(new Spark(kLeftMotorPort), + new Spark(kRightMotorPort)); + + /** + * Tells the robot to drive to a set distance (in inches) from an object + * using proportional control. + */ + public void teleopPeriodic() { + // sensor returns a value from 0-4095 that is scaled to inches + double currentDistance = m_ultrasonic.getValue() * kValueToInches; + + // convert distance error to a motor speed + double currentSpeed = (kHoldDistance - currentDistance) * kP; + + // drive robot + m_robotDrive.arcadeDrive(currentSpeed, 0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java new file mode 100644 index 0000000000..1d49da4bba --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.ultrasonicpid; + +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; + +/** + * This is a sample program to demonstrate the use of a PIDController with an + * ultrasonic sensor to reach and maintain a set distance from an object. + */ +public class Robot extends IterativeRobot { + // distance in inches the robot wants to stay from an object + private static final double kHoldDistance = 12.0; + + // maximum distance in inches we expect the robot to see + private static final double kMaxDistance = 24.0; + + // factor to convert sensor values to a distance in inches + private static final double kValueToInches = 0.125; + + // proportional speed constant + private static final double kP = 7.0; + + // integral speed constant + private static final double kI = 0.018; + + // derivative speed constant + private static final double kD = 1.5; + + private static final int kLeftMotorPort = 0; + private static final int kRightMotorPort = 1; + private static final int kUltrasonicPort = 0; + + private AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort); + private DifferentialDrive m_robotDrive + = new DifferentialDrive(new Spark(kLeftMotorPort), + new Spark(kRightMotorPort)); + private PIDController m_pidController + = new PIDController(kP, kI, kD, m_ultrasonic, new MyPidOutput()); + + /** + * Drives the robot a set distance from an object using PID control and the + * ultrasonic sensor. + */ + @Override + public void teleopInit() { + // Set expected range to 0-24 inches; e.g. at 24 inches from object go + // full forward, at 0 inches from object go full backward. + m_pidController.setInputRange(0, kMaxDistance * kValueToInches); + // Set setpoint of the pid controller + m_pidController.setSetpoint(kHoldDistance * kValueToInches); + m_pidController.enable(); // begin PID control + } + + private class MyPidOutput implements PIDOutput { + @Override + public void pidWrite(double output) { + m_robotDrive.arcadeDrive(output, 0); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/axiscamera/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/axiscamera/Robot.java new file mode 100755 index 0000000000..47e28295ae --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/axiscamera/Robot.java @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.vision.axiscamera; + +import edu.wpi.cscore.AxisCamera; +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.CvSource; +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.IterativeRobot; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +/** + * This is a demo program showing the use of OpenCV to do vision processing. The + * image is acquired from the Axis camera, then a rectangle is put on the image + * and sent to the dashboard. OpenCV has many methods for different types of + * processing. + */ +public class Robot extends IterativeRobot { + + Thread m_visionThread; + + @Override + public void robotInit() { + m_visionThread = new Thread(() -> { + // Get the Axis camera from CameraServer + AxisCamera camera + = CameraServer.getInstance().addAxisCamera("axis-camera.local"); + // Set the resolution + camera.setResolution(640, 480); + + // Get a CvSink. This will capture Mats from the camera + CvSink cvSink = CameraServer.getInstance().getVideo(); + // Setup a CvSource. This will send images back to the Dashboard + CvSource outputStream + = CameraServer.getInstance().putVideo("Rectangle", 640, 480); + + // Mats are very memory expensive. Lets reuse this Mat. + Mat mat = new Mat(); + + // This cannot be 'true'. The program will never exit if it is. This + // lets the robot stop this thread when restarting robot code or + // deploying. + while (!Thread.interrupted()) { + // Tell the CvSink to grab a frame from the camera and put it + // in the source mat. If there is an error notify the output. + if (cvSink.grabFrame(mat) == 0) { + // Send the output the error. + outputStream.notifyError(cvSink.getError()); + // skip the rest of the current iteration + continue; + } + // Put a rectangle on the image + Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400), + new Scalar(255, 255, 255), 5); + // Give the output stream a new image to display + outputStream.putFrame(mat); + } + }); + m_visionThread.setDaemon(true); + m_visionThread.start(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/intermediatevision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/intermediatevision/Robot.java new file mode 100755 index 0000000000..ae2caa200f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/intermediatevision/Robot.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.vision.intermediatevision; + +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.CvSource; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.IterativeRobot; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +/** + * This is a demo program showing the use of OpenCV to do vision processing. The + * image is acquired from the USB camera, then a rectangle is put on the image + * and sent to the dashboard. OpenCV has many methods for different types of + * processing. + */ +public class Robot extends IterativeRobot { + + Thread m_visionThread; + + @Override + public void robotInit() { + m_visionThread = new Thread(() -> { + // Get the UsbCamera from CameraServer + UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); + // Set the resolution + camera.setResolution(640, 480); + + // Get a CvSink. This will capture Mats from the camera + CvSink cvSink = CameraServer.getInstance().getVideo(); + // Setup a CvSource. This will send images back to the Dashboard + CvSource outputStream + = CameraServer.getInstance().putVideo("Rectangle", 640, 480); + + // Mats are very memory expensive. Lets reuse this Mat. + Mat mat = new Mat(); + + // This cannot be 'true'. The program will never exit if it is. This + // lets the robot stop this thread when restarting robot code or + // deploying. + while (!Thread.interrupted()) { + // Tell the CvSink to grab a frame from the camera and put it + // in the source mat. If there is an error notify the output. + if (cvSink.grabFrame(mat) == 0) { + // Send the output the error. + outputStream.notifyError(cvSink.getError()); + // skip the rest of the current iteration + continue; + } + // Put a rectangle on the image + Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400), + new Scalar(255, 255, 255), 5); + // Give the output stream a new image to display + outputStream.putFrame(mat); + } + }); + m_visionThread.setDaemon(true); + m_visionThread.start(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/quickvision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/quickvision/Robot.java new file mode 100755 index 0000000000..236b3a6661 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/vision/quickvision/Robot.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.vision.quickvision; + +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.IterativeRobot; + +/** + * Uses the CameraServer class to automatically capture video from a USB webcam + * and send it to the FRC dashboard without doing any vision processing. This + * is the easiest way to get camera images to the dashboard. Just add this to + * the robotInit() method in your program. + */ +public class Robot extends IterativeRobot { + + @Override + public void robotInit() { + CameraServer.getInstance().startAutomaticCapture(); + } + +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java new file mode 100644 index 0000000000..68499ef452 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased; + +/** + * This class is the glue that binds the controls on the physical operator + * interface to the commands and command groups that allow control of the robot. + */ +public class OI { + //// CREATING BUTTONS + // One type of button is a joystick button which is any button on a + //// joystick. + // You create one by telling it which joystick it's on and which button + // number it is. + // Joystick stick = new Joystick(port); + // Button button = new JoystickButton(stick, buttonNumber); + + // There are a few additional built in buttons you can use. Additionally, + // by subclassing Button you can create custom triggers and bind those to + // commands the same as any other Button. + + //// TRIGGERING COMMANDS WITH BUTTONS + // Once you have a button, it's trivial to bind it to a button in one of + // three ways: + + // Start the command when the button is pressed and let it run the command + // until it is finished as determined by it's isFinished method. + // button.whenPressed(new ExampleCommand()); + + // Run the command while the button is being held down and interrupt it once + // the button is released. + // button.whileHeld(new ExampleCommand()); + + // Start the command when the button is released and let it run the command + // until it is finished as determined by it's isFinished method. + // button.whenReleased(new ExampleCommand()); +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java new file mode 100644 index 0000000000..c6173543aa --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java @@ -0,0 +1,124 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand; +import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the IterativeRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.properties file in the + * project. + */ +public class Robot extends IterativeRobot { + + public static final ExampleSubsystem kExampleSubsystem + = new ExampleSubsystem(); + public static OI m_oi; + + Command m_autonomousCommand; + SendableChooser m_chooser = new SendableChooser<>(); + + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + m_oi = new OI(); + m_chooser.addDefault("Default Auto", new ExampleCommand()); + // chooser.addObject("My Auto", new MyAutoCommand()); + SmartDashboard.putData("Auto mode", m_chooser); + } + + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ + @Override + public void disabledInit() { + + } + + @Override + public void disabledPeriodic() { + Scheduler.getInstance().run(); + } + + /** + * 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 code to get the auto name from the text box below the Gyro + * + *

You can add additional auto modes by adding additional commands to the + * chooser code above (like the commented example) or additional comparisons + * to the switch structure below with additional strings & commands. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_chooser.getSelected(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.start(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + Scheduler.getInstance().run(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + LiveWindow.run(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java new file mode 100644 index 0000000000..32149f88ee --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased; + +/** + * The RobotMap is a mapping from the ports sensors and actuators are wired into + * to a variable name. This provides flexibility changing wiring, makes checking + * the wiring easier and significantly reduces the number of magic numbers + * floating around. + */ +public class RobotMap { + // For example to map the left and right motors, you could define the + // following variables to use with your drivetrain subsystem. + // public static int leftMotor = 1; + // public static int rightMotor = 2; + + // If you are using multiple modules, make sure to define both the port + // number and the module. For example you with a rangefinder: + // public static int rangefinderPort = 1; + // public static int rangefinderModule = 1; +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java new file mode 100644 index 0000000000..4913917a1c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.templates.commandbased.Robot; + +/** + * An example command. You can replace me with your own command. + */ +public class ExampleCommand extends Command { + public ExampleCommand() { + // Use requires() here to declare subsystem dependencies + requires(Robot.kExampleSubsystem); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java new file mode 100644 index 0000000000..f61f306777 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased.subsystems; + +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * An example subsystem. You can replace me with your own Subsystem. + */ +public class ExampleSubsystem extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java new file mode 100644 index 0000000000..1eee908215 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.iterative; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the IterativeRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.properties file in the + * project. + */ +public class Robot extends IterativeRobot { + + private static final String kDefaultAuto = "Default"; + private static final String kCustomAuto = "My Auto"; + private String m_autoSelected; + private SendableChooser m_chooser = new SendableChooser<>(); + + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + m_chooser.addDefault("Default Auto", kDefaultAuto); + m_chooser.addObject("My Auto", kCustomAuto); + SmartDashboard.putData("Auto choices", m_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 switch structure below with additional strings. If using the + * SendableChooser make sure to add them to the chooser code above as well. + */ + @Override + public void autonomousInit() { + m_autoSelected = m_chooser.getSelected(); + // autoSelected = SmartDashboard.getString("Auto Selector", + // defaultAuto); + System.out.println("Auto selected: " + m_autoSelected); + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + switch (m_autoSelected) { + case kCustomAuto: + // Put custom auto code here + break; + case kDefaultAuto: + default: + // Put default auto code here + break; + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java new file mode 100644 index 0000000000..9b64e47d93 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java @@ -0,0 +1,146 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.sample; + +import edu.wpi.first.wpilibj.SampleRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * 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 build.properties file in the + * project. + * + *

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 String kDefaultAuto = "Default"; + private static final String kCustomAuto = "My Auto"; + + private DifferentialDrive m_robotDrive + = new DifferentialDrive(new Spark(0), new Spark(1)); + private Joystick m_stick = new Joystick(0); + private SendableChooser m_chooser = new SendableChooser<>(); + + public Robot() { + m_robotDrive.setExpiration(0.1); + } + + @Override + public void robotInit() { + m_chooser.addDefault("Default Auto", kDefaultAuto); + m_chooser.addObject("My Auto", kCustomAuto); + SmartDashboard.putData("Auto modes", m_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. + * + *

If you wanted to run a similar autonomous mode with an IterativeRobot + * you would write: + * + * 

{@code
+	 * Timer timer = new Timer();
+	 *
+	 * // This function is run once each time the robot enters autonomous mode
+	 * public void autonomousInit() {
+	 *     timer.reset();
+	 *     timer.start();
+	 * }
+	 *
+	 * // This function is called periodically during autonomous
+	 * public void autonomousPeriodic() {
+	 * // Drive for 2 seconds
+	 *     if (timer.get() < 2.0) {
+	 *         myRobot.drive(-0.5, 0.0); // drive forwards half speed
+	 *     } else if (timer.get() < 5.0) {
+	 *         myRobot.drive(-1.0, 0.0); // drive forwards full speed
+	 *     } else {
+	 *         myRobot.drive(0.0, 0.0); // stop robot
+	 *     }
+	 * }
+	 * }
+ */ + @Override + public void autonomous() { + String autoSelected = m_chooser.getSelected(); + // String autoSelected = SmartDashboard.getString("Auto Selector", + // defaultAuto); + System.out.println("Auto selected: " + autoSelected); + + switch (autoSelected) { + case kCustomAuto: + m_robotDrive.setSafetyEnabled(false); + m_robotDrive.arcadeDrive(-0.5, 1.0); // spin at half speed + Timer.delay(2.0); // for 2 seconds + m_robotDrive.arcadeDrive(0.0, 0.0); // stop robot + break; + case kDefaultAuto: + default: + m_robotDrive.setSafetyEnabled(false); + m_robotDrive.arcadeDrive(-0.5, 0.0); // drive forwards + Timer.delay(2.0); // for 2 seconds + m_robotDrive.arcadeDrive(0.0, 0.0); // stop robot + break; + } + } + + /** + * Runs the motors with arcade steering. + * + *

If you wanted to run a similar teleoperated mode with an IterativeRobot + * you would write: + * + * 

{@code
+	 * // This function is called periodically during operator control
+	 * public void teleopPeriodic() {
+	 *     myRobot.arcadeDrive(stick);
+	 * }
+	 * }
+ */ + @Override + public void operatorControl() { + m_robotDrive.setSafetyEnabled(true); + while (isOperatorControl() && isEnabled()) { + // drive arcade style + m_robotDrive.arcadeDrive(m_stick.getX(), m_stick.getY()); + // wait for a motor update time + Timer.delay(0.005); + } + } + + /** + * Runs during test mode. + */ + @Override + public void test() { + } +}