RoboRealm V3 GearDrop VB Script

Transcription

1 RoboRealm V3 GearDrop VB Script height = GetVariable("HEIGHT") width = GetVariable("WIDTH") cogx = GetVariable("COG_X") cogx_tol = 5 height_sweet = 461 strafe_multiplier = 2.0 area = height * width if area > 2000 then SetVariable "TARGET_AREA", area targetcogx = 1.208E 7 * area * area 0.02 * area if Abs(targetCogX cogx) < cogx_tol then targetcogx = cogx end if targetheight = 100*(height_sweet height)/(height_sweet) SetVariable "TARGET_CogX", targetcogx SetVariable "TARGET_Height", targetheight forward = (height_sweet height)/(height_sweet*2) strafe = (cogx targetcogx)/(1280) * strafe_multiplier speed = sqr(forward*forward+strafe*strafe) if forward > 0 then direction = Atn(strafe/forward)*180/ else speed = 0 direction = 0 SetVariable "ATTARGET", TRUE end if SetVariable "ATTARGET", FALSE SetVariable "FORWARD", Round(forward,3) SetVariable "STRAFE", Round(strafe,3) SetVariable "SPEED", Round(speed,3) SetVariable "REL_DIRECTION", Round(direction,3) else SetVariable "ATTARGET", FALSE SetVariable "TARGET_AREA", 0 SetVariable "TARGET_CogX", 0

2 end if SetVariable "TARGET_Height", 0 SetVariable "FORWARD", 0 SetVariable "STRAFE", 0 SetVariable "SPEED", 0 SetVariable "DIRECTION", 0 SetVariable "REL_DIRECTION", 0

3 RobotMap.java package org.usfirst.frc.team3011.robot; * 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 { //swerve code constants public static final double angletol = 5.0, width = 25, length = 22.5, yawcorrection = 0.30, //0.33 is to much yawmultiplier = 2.5, yawtol = 1; //auto constants public static final double timetosearch = 3.5, leftautoforwardtime = 1.3, rightautoforwardtime = 1.35, firstboilermovetime = 0.5, firstboilermovespeed = 0.5, secondboilermovetime = 0.5, secondboilermovespeed = 0.5, boilertimertogear = 5, timetoshoot = 2; //wench constants public static final double wenchspeed = 1.0; /***THIS NUMBER MUST BE NEGATIVE!!!!!**** //shooter/feeder variables public static final double feederspeed = 0.5, feedershootspeed = 0.5,

4 conveyerfeedspeed = 0.7, conveyershootspeed = 0.7, shooterspeed = 0.35; //joystick zero tolerence public static final double speedtol = 0.2, rightxtol = 0.2, leftxtol = 0.2, leftytol = 0.2, turbotol = 0.2; //speed multiplier public static final double speedmultiplier = 0.6; //pot encoder 0 values public static final double leftfronthome = 0.37, leftrearhome = 3.54, rightfronthome = 2.93, rightrearhome = 2.16; //air pressure information public static final int compressorcan = 10, actuatorone = 3; //can talons public static final int //the outside motor is always motor 1 leftfrontdrive1 = 1, //t4 leftfrontdrive2 = 4, //t3 leftreardrive1 = 3, //t2 leftreardrive2 = 5, //t1 rightfrontdrive1 = 6, //t5 rightfrontdrive2 = 8, //t6 rightreardrive1 = 7, //t8 rightreardrive2 = 2; //t7

5 //victors public static final int leftfrontturn = 2, //v1 leftrearturn = 3, //v0 rightfrontturn = 6, //v2 rightrearturn = 7, //v3 feeder = 5, wench = 4, shooter = 8, conveyer = 9; //encoders public static final int leftfront = 1, leftrear = 0, rightfront = 2, rightrear = 3;

6 IO.java package org.usfirst.frc.team3011.robot; import edu.wpi.first.wpilibj.joystick; import edu.wpi.first.wpilibj.buttons.button; import edu.wpi.first.wpilibj.buttons.joystickbutton; import org.usfirst.frc.team3011.robot.commands.resetgyro; import org.usfirst.frc.team3011.robot.commands.shootballs; import org.usfirst.frc.team3011.robot.commands.wench; import org.usfirst.frc.team3011.robot.commands.gotoyaw; import org.usfirst.frc.team3011.robot.commands.autogear; import org.usfirst.frc.team3011.robot.commands.automove; * 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 { public Joystick driverpad = new Joystick(0); private Button buttonstart = new JoystickButton(driverPad,8); private Button buttonback = new JoystickButton(driverPad,7); private Button buttony = new JoystickButton(driverPad,4); private Button buttonx = new JoystickButton(driverPad,3); private Button buttonb = new JoystickButton(driverPad,2); private Button buttona = new JoystickButton(driverPad,1); private Button buttonrb = new JoystickButton(driverPad,6); private Button buttonlb = new JoystickButton(driverPad,5); public OI() { this.buttonback.whenpressed(new ResetGyro()); this.buttonstart.whenpressed(new ShootBalls(RobotMap.timeToShoot)); this.buttonrb.whenpressed(new Wench()); this.buttonx.whenpressed(new GoToYaw(60)); this.buttonb.whenpressed(new GoToYaw( 60)); this.buttony.whenpressed(new AutoGear(3)); this.buttona.whenpressed(new GoToYaw(0)); //this.buttonlb.whenpressed(new GoToYaw(180)); this.buttonlb.whenpressed(new Vacuum());

7 //im on the blue team //this.buttona.whenpressed(new GoToYaw(45)); //this.buttona.whenpressed(new AutoMove(0.5,0,60,2.2)); //im on the red team //this.buttona.whenpressed(new GoToYaw( 45)); //// 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());

8 Robot.java package org.usfirst.frc.team3011.robot; import org.usfirst.frc.team3011.robot.commands.autonomousleft; import org.usfirst.frc.team3011.robot.commands.autonomousright; //import org.usfirst.frc.team3011.robot.commands.examplecommand; import org.usfirst.frc.team3011.robot.subsystems.examplesubsystem; import org.usfirst.frc.team3011.robot.subsystems.ss_drivetrain; import edu.wpi.first.wpilibj.cameraserver; import edu.wpi.first.wpilibj.driverstation; 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.sendablechooser; import edu.wpi.first.wpilibj.smartdashboard.smartdashboard; import com.kauailabs.navx.frc.ahrs; import edu.wpi.first.wpilibj.spi; import edu.wpi.first.wpilibj.timer; import edu.wpi.first.wpilibj.victor; * 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 { public static final ExampleSubsystem examplesubsystem = new ExampleSubsystem(); public static final SS_DriveTrain drivetrain = new SS_DriveTrain(); public static OI oi;

9 public static AHRS ahrs; public static final Victor feeder = new Victor(RobotMap.feeder); public static final Victor conveyer = new Victor(RobotMap.conveyer); public static final Victor wench = new Victor(RobotMap.wench); public static final Victor shooter = new Victor(RobotMap.shooter); public static boolean wrenchon = false; public static boolean shooting = false; public static Command autonomouscommand; //public static SendableChooser chooser; * This function is run when the robot is first started up and should be * used for any initialization code. public void robotinit() { //navx code ************************* try { /* Communicate w/navx MXP via the MXP SPI Bus. /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB /* See mxp.kauailabs.com/guidance/selecting an interface/ for details. ahrs = new AHRS(SPI.Port.kMXP); catch (RuntimeException ex ) { DriverStation.reportError("Error instantiating navx MXP: " + ex.getmessage(), true); while (ahrs.iscalibrating()) { if (!ahrs.iscalibrating() ) { Timer.delay( 0.3 ); ahrs.zeroyaw(); /********************************* Robot.oi = new OI();

10 CameraServer.getInstance().startAutomaticCapture(); //Robot.chooser = new SendableChooser(); //Robot.chooser.addDefault("Left", new AutonomousLeft()); //chooser.addobject("right", new AutonomousLeft()); //SmartDashboard.putData("Auto mode", 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. public void disabledinit(){ 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. public void autonomousinit() { //autonomouscommand = (Command) chooser.getselected(); //String autoselected = SmartDashboard.getString("Auto Selector", "Default"); //switch(autoselected) { // case "Left": // autonomouscommand = new AutonomousLeft(); // break; // case "Right": // autonomouscommand = new AutonomousLeft();

11 // break; // default: // autonomouscommand = new AutonomousLeft(); // break; // Robot.ahrs.reset(); Timer.delay(0.5); autonomouscommand = new AutonomousLeft(); //autonomouscommand = new AutonomousRight(); // schedule the autonomous command (example) if (autonomouscommand!= null) autonomouscommand.start(); * This function is called periodically during autonomous public void autonomousperiodic() { Scheduler.getInstance().run(); 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 (autonomouscommand!= null) autonomouscommand.cancel(); SmartDashboard.putString("RECORDER", "Stop"); SmartDashboard.putBoolean("RR_ATTARGET", false); Robot.driveTrain.allGoHome(); //Robot.feeder.set(RobotMap.feederSpeed); //Robot.conveyer.set(RobotMap.conveyerFeedSpeed); * This function is called periodically during operator control public void teleopperiodic() {

12 Scheduler.getInstance().run(); Robot.driveTrain.swerveWheel( oi.driverpad.getrawaxis(1), oi.driverpad.getrawaxis(0), oi.driverpad.getrawaxis(4), oi.driverpad.getrawaxis(2)); /* Display 6 axis Processed Angle Data //SmartDashboard.putBoolean( "IMU_Connected", ahrs.isconnected()); //SmartDashboard.putBoolean( "IMU_IsCalibrating", ahrs.iscalibrating()); SmartDashboard.putNumber( "IMU_Yaw", ahrs.getyaw()); //SmartDashboard.putNumber( "IMU_Pitch", ahrs.getpitch()); //SmartDashboard.putNumber( "IMU_Roll", ahrs.getroll()); /* Display tilt corrected, Magnetometer based heading (requires /* magnetometer calibration to be useful) //SmartDashboard.putNumber( "IMU_CompassHeading", ahrs.getcompassheading()); /* Display 9 axis Heading (requires magnetometer calibration to be useful) //SmartDashboard.putNumber( "IMU_FusedHeading", ahrs.getfusedheading()); /* These functions are compatible w/the WPI Gyro Class, providing a simple /* path for upgrading from the Kit of Parts gyro to the navx MXP //SmartDashboard.putNumber( "IMU_TotalYaw", ahrs.getangle()); //SmartDashboard.putNumber( "IMU_YawRateDPS", ahrs.getrate()); /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) //SmartDashboard.putNumber( "IMU_Accel_X", ahrs.getworldlinearaccelx()); //SmartDashboard.putNumber( "IMU_Accel_Y", ahrs.getworldlinearaccely()); //SmartDashboard.putBoolean( "IMU_IsMoving", ahrs.ismoving()); //SmartDashboard.putBoolean( "IMU_IsRotating", ahrs.isrotating()); /* Display estimates of velocity/displacement. Note that these values are /* not expected to be accurate enough for estimating robot position on a /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding /* of these errors due to single (velocity) integration and especially /* double (displacement) integration.

13 //SmartDashboard.putNumber( "Velocity_X", ahrs.getvelocityx()); //SmartDashboard.putNumber( "Velocity_Y", ahrs.getvelocityy()); //SmartDashboard.putNumber( "Displacement_X", ahrs.getdisplacementx()); //SmartDashboard.putNumber( "Displacement_Y", ahrs.getdisplacementy()); /* Display Raw Gyro/Accelerometer/Magnetometer Values /* NOTE: These values are not normally necessary, but are made available /* for advanced users. Before using this data, please consider whether /* the processed data (see above) will suit your needs. //SmartDashboard.putNumber( "RawGyro_X", ahrs.getrawgyrox()); //SmartDashboard.putNumber( "RawGyro_Y", ahrs.getrawgyroy()); //SmartDashboard.putNumber( "RawGyro_Z", ahrs.getrawgyroz()); //SmartDashboard.putNumber( "RawAccel_X", ahrs.getrawaccelx()); //SmartDashboard.putNumber( "RawAccel_Y", ahrs.getrawaccely()); //SmartDashboard.putNumber( "RawAccel_Z", ahrs.getrawaccelz()); //SmartDashboard.putNumber( "RawMag_X", ahrs.getrawmagx()); //SmartDashboard.putNumber( "RawMag_Y", ahrs.getrawmagy()); //SmartDashboard.putNumber( "RawMag_Z", ahrs.getrawmagz()); //SmartDashboard.putNumber( "IMU_Temp_C", ahrs.gettempc()); /* Omnimount Yaw Axis Information /* For more info, see mxp.kauailabs.com/installation/omnimount //AHRS.BoardYawAxis yaw_axis = ahrs.getboardyawaxis(); //SmartDashboard.putString( "YawAxisDirection", yaw_axis.up? "Up" : "Down" ); //SmartDashboard.putNumber( "YawAxis", yaw_axis.board_axis.getvalue() ); /* Sensor Board Information //SmartDashboard.putString( "FirmwareVersion", ahrs.getfirmwareversion()); /* Quaternion Data /* Quaternions are fascinating, and are the most compact representation of /* orientation data. All of the Yaw, Pitch and Roll Values can be derived /* from the Quaternions. If interested in motion processing, knowledge of /* Quaternions is highly recommended. //SmartDashboard.putNumber( "QuaternionW", ahrs.getquaternionw()); //SmartDashboard.putNumber( "QuaternionX", ahrs.getquaternionx()); //SmartDashboard.putNumber( "QuaternionY", ahrs.getquaterniony()); //SmartDashboard.putNumber( "QuaternionZ", ahrs.getquaternionz());

14 /* Connectivity Debugging Support //SmartDashboard.putNumber( "IMU_Byte_Count", ahrs.getbytecount()); //SmartDashboard.putNumber( "IMU_Update_Count", ahrs.getupdatecount()); * This function is called periodically during test mode public void testperiodic() { LiveWindow.run();

15 AutoGear.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robot; import edu.wpi.first.wpilibj.command.command; import edu.wpi.first.wpilibj.smartdashboard.smartdashboard; * public class AutoGear extends Command { private double yawhold = 0; private double speed = 0; public AutoGear(double timeout) { settimeout(timeout); // Called just before this Command runs the first time protected void initialize() { Robot.driveTrain.setAutoOn(true); SmartDashboard.putString("RECORDER", "Record"); this.yawhold = Robot.driveTrain.getYawHold(); // Called repeatedly when this Command is scheduled to run protected void execute() { //Robot.driveTrain.swerveWheelAuto(this.lateralSpeed,this.lateralAngle); this.speed = SmartDashboard.getNumber("RR_SPEED",0); double angle = SmartDashboard.getNumber("RR_REL_DIRECTION",0); Robot.driveTrain.swerveWheelAuto(speed, angle+this.yawhold); // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { if (SmartDashboard.getBoolean("RR_ATTARGET",false)) {

16 return true; return istimedout(); // Called once after isfinished returns true protected void end() { Robot.driveTrain.setAutoOn(false); Robot.driveTrain.allGoHome(); SmartDashboard.putString("RECORDER", "Stop"); // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() {

17 AutoMove.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robot; import edu.wpi.first.wpilibj.timer; import edu.wpi.first.wpilibj.command.command; import edu.wpi.first.wpilibj.smartdashboard.smartdashboard; * public class AutoMove extends Command { private double lateralspeed; private double lateralangle; private double setyaw; public AutoMove(double lspeed, double langle, double syaw, double timeout) { this.lateralspeed = lspeed; this.lateralangle = langle; this.setyaw = syaw; settimeout(timeout); // Called just before this Command runs the first time protected void initialize() { Robot.driveTrain.setAutoOn(true); Robot.driveTrain.setYaw(this.setYaw); Robot.driveTrain.swerveWheelAuto(this.lateralSpeed, this.lateralangle); // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.driveTrain.swerveWheelAuto(this.lateralSpeed,this.lateralAngle);

18 // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { return istimedout(); // Called once after isfinished returns true protected void end() { Robot.driveTrain.setAutoOn(false); Robot.driveTrain.allGoHome(); // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() {

19 AutonomousLeft.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robotmap; import edu.wpi.first.wpilibj.command.commandgroup; * public class AutonomousLeft extends CommandGroup { public AutonomousLeft() { addsequential(new AutoMove(0.5,0,0,.5)); addsequential(new AutoMove(0.5,0,60,RobotMap.leftAutoForwardTime)); addsequential(new AutoGear(RobotMap.timeToSearch)); // Add Commans here: // e.g. addsequential(new Command1()); // addsequential(new Command2()); // these will run in order. // To run multiple commands at the same time, // use addparallel() // e.g. addparallel(new Command1()); // addsequential(new Command2()); // Command1 and Command2 will run in parallel. // A command group will require all of the subsystems that each member // would require. // e.g. if Command1 requires chassis, and Command2 requires arm, // a CommandGroup containing them would require both the chassis and the // arm.

20 AutonomousRight.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robotmap; import edu.wpi.first.wpilibj.command.commandgroup; * public class AutonomousRight extends CommandGroup { public AutonomousRight() { addsequential(new AutoMove(0.5,0,0,.5)); addsequential(new AutoMove(0.5,0, 60,RobotMap.rightAutoForwardTime)); addsequential(new AutoGear(RobotMap.timeToSearch)); // Add Commands here: // e.g. addsequential(new Command1()); // addsequential(new Command2()); // these will run in order. // To run multiple commands at the same time, // use addparallel() // e.g. addparallel(new Command1()); // addsequential(new Command2()); // Command1 and Command2 will run in parallel. // A command group will require all of the subsystems that each member // would require. // e.g. if Command1 requires chassis, and Command2 requires arm, // a CommandGroup containing them would require both the chassis and the // arm.

21 AutonomousRightBoiler.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robotmap; import edu.wpi.first.wpilibj.command.commandgroup; * public class AutonomousRightBoiler extends CommandGroup { public AutonomousRightBoiler() { //this is the move to place the robot's butt infront of the hole //addsequential(new AutoMove(RobotMap.firstBoilerMoveSpeed, 45, 45, RobotMap.firstBoilerMoveTime)); addsequential(new ShootBalls(RobotMap.timeToShoot); //this move gets the butt off the boiler wall addsequential(new AutoMove(RobotMap.secondBoilerMoveSpeed, 20, 60, RobotMap.secondBoilerMoveTime)); addsequential(new AutoGear(RobotMap.boilerTimerToGear)); //addsequential(new AutoMove(0.5,0,0,.5)); //addsequential(new AutoMove(0.5,0, 60,RobotMap.rightAutoForwardTime)); //addsequential(new AutoGear(RobotMap.timeToSearch)); // Add Commands here: // e.g. addsequential(new Command1()); // addsequential(new Command2()); // these will run in order. // To run multiple commands at the same time, // use addparallel() // e.g. addparallel(new Command1()); // addsequential(new Command2()); // Command1 and Command2 will run in parallel. // A command group will require all of the subsystems that each member // would require. // e.g. if Command1 requires chassis, and Command2 requires arm, // a CommandGroup containing them would require both the chassis and the

22 // arm.

23 GoToYaw.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robot; import edu.wpi.first.wpilibj.timer; import edu.wpi.first.wpilibj.command.command; * public class GoToYaw extends Command { private double newyaw; public GoToYaw(double input) { this.newyaw = input; // Called just before this Command runs the first time protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.driveTrain.setYaw(this.newYaw); // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { return true; // Called once after isfinished returns true protected void end() { // Called when another command which requires one or more of the same

24 // subsystems is scheduled to run protected void interrupted() {

25 ResetGyro.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robot; import edu.wpi.first.wpilibj.timer; import edu.wpi.first.wpilibj.command.command; * public class ResetGyro extends Command { public ResetGyro() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); // Called just before this Command runs the first time protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.ahrs.reset(); Timer.delay(2); // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { return true; // Called once after isfinished returns true protected void end() {

26 // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() {

27 RotateTo.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robot; import edu.wpi.first.wpilibj.command.command; * public class RotateTo extends Command { private double rotateto; public RotateTo(double a) { this.rotateto = a; // Use requires() here to declare subsystem dependencies // eg. requires(chassis); // Called just before this Command runs the first time protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.driveTrain.setYaw(this.rotateTo); // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { return true; // Called once after isfinished returns true protected void end() {

28 // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() {

29 ShootBalls.java import edu.wpi.first.wpilibj.timer; import edu.wpi.first.wpilibj.command.command; * public class ShootBalls extends Command { public ShootBalls(double timeout) { settimeout(timeout); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); // Called just before this Command runs the first time protected void initialize() { Robot.shooter.set(RobotMap.shooterSpeed); Robot.feeder.set(RobotMap.feederShootSpeed); Robot.conveyer.set(RobotMap.conveyerShootSpeed); /* if (!Robot.shooting) { Robot.shooting = true; Robot.shooter.set(RobotMap.shooterSpeed); Robot.feeder.set(RobotMap.feederShootSpeed); //Robot.conveyer.set(0); //Timer.delay(1); Robot.conveyer.set(RobotMap.conveyerShootSpeed); else { Robot.shooting = false; Robot.conveyer.set(0); Robot.shooter.set(0); //Robot.feeder.set(RobotMap.feederSpeed); //Timer.delay(0.2); //Robot.conveyer.set(RobotMap.conveyerFeedSpeed);

30 // Called repeatedly when this Command is scheduled to run protected void execute() { // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { return istimedout(); // Called once after isfinished returns true protected void end() { Robot.conveyer.set(0); Robot.shooter.set(0); Robot.feeder.set(0); // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() {

31 Wench.java package org.usfirst.frc.team3011.robot.commands; import org.usfirst.frc.team3011.robot.robot; import org.usfirst.frc.team3011.robot.robotmap; import edu.wpi.first.wpilibj.command.command; * public class Wench extends Command { public Wench() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); // Called just before this Command runs the first time protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { if (Robot.wrenchOn) { Robot.wench.set(0); Robot.wrenchOn = false; else { Robot.wench.set(RobotMap.wenchSpeed); Robot.wrenchOn = true; // Make this return true when this Command no longer needs to run execute() protected boolean isfinished() { return true;

32 // Called once after isfinished returns true protected void end() { // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() {