From 877b7aaf8d6a1a7b0a77405bc80ea9367fe6eb5c Mon Sep 17 00:00:00 2001 From: Adam Long Date: Mon, 12 Sep 2016 21:42:04 +0000 Subject: [PATCH] added roller code --- src/org/usfirst/frc/team2059/robot/OI.java | 5 ++- src/org/usfirst/frc/team2059/robot/Robot.java | 3 ++ .../usfirst/frc/team2059/robot/RobotMap.java | 2 +- .../team2059/robot/commands/CommandBase.java | 3 ++ .../team2059/robot/commands/ShootAtSpeed.java | 34 +++++++++++++++++ .../team2059/robot/commands/SpinRollers.java | 38 +++++++++++++++++++ .../team2059/robot/subsystems/Shooter.java | 15 ++++++++ 7 files changed, 98 insertions(+), 2 deletions(-) create mode 100644 src/org/usfirst/frc/team2059/robot/commands/ShootAtSpeed.java create mode 100644 src/org/usfirst/frc/team2059/robot/commands/SpinRollers.java create mode 100644 src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 89fd783..5a3cddb 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -8,6 +8,8 @@ import org.usfirst.frc.team2059.robot.commands.MoveArm; import org.usfirst.frc.team2059.robot.commands.SetArmPosition; import org.usfirst.frc.team2059.robot.commands.SetShooterState; import org.usfirst.frc.team2059.robot.commands.SetArmStopState; +import org.usfirst.frc.team2059.robot.commands.ShootAtSpeed; +import org.usfirst.frc.team2059.robot.commands.SpinRollers; import org.usfirst.frc.team2059.robot.RobotMap; /** * This class is the glue that binds the controls on the physical operator @@ -29,7 +31,8 @@ public class OI { } // Print log when button 1 pressed //joystickButtons[0][0].whenPressed(new LogEncoder()); - joystickButtons[0][0].whileHeld(new SetShooterState(true)); + joystickButtons[0][0].whileHeld(new SpinRollers(1)); + joystickButtons[0][1].whileHeld(new SetShooterState(true)); joystickButtons[1][0].whileHeld(new MoveArm(0.5)); joystickButtons[1][1].whileHeld(new MoveArm(-0.5)); diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index 7487212..ab15450 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -17,6 +17,8 @@ public class Robot extends IterativeRobot { SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController()); SmartDashboard.putBoolean("CompressorEnabled",true); + //Automatically determine if rolling in or rolling out + SmartDashboard.putBoolean("SmartRollers",true); } public void disabledInit() { } @@ -37,6 +39,7 @@ public class Robot extends IterativeRobot { autonomousCommand.cancel(); } CommandBase.pneumatics.setCompressorEnabled(true); + CommandBase.pneumatics.setArmStopState(false); } public void teleopPeriodic() { Scheduler.getInstance().run(); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 88916fd..4c82f66 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -25,7 +25,7 @@ public class RobotMap { public static int armStopSolenoidOne = 4; public static int armStopSolenoidTwo = 5; //Misc - public static int mainArmPresetCollect = 0; + public static int mainArmPresetCollect = -3; public static int mainArmPresetTraverse = 11; public static int mainArmPresetCloseShot = 90; public static int mainArmPresetFarShot = 70; diff --git a/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java b/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java index 8677a4b..1dacda0 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java +++ b/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java @@ -3,17 +3,20 @@ import org.usfirst.frc.team2059.robot.subsystems.DriveBase; import org.usfirst.frc.team2059.robot.subsystems.EncoderBase; import org.usfirst.frc.team2059.robot.subsystems.MainArm; import org.usfirst.frc.team2059.robot.subsystems.Pneumatics; +import org.usfirst.frc.team2059.robot.subsystems.Shooter; import edu.wpi.first.wpilibj.command.Command; public abstract class CommandBase extends Command { public static EncoderBase encoderBase; public static DriveBase driveBase; public static MainArm mainArm; public static Pneumatics pneumatics; + public static Shooter shooter; public static void init() { encoderBase = new EncoderBase(); driveBase = new DriveBase(); mainArm = new MainArm(); pneumatics = new Pneumatics(); + shooter = new Shooter(); } } // vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/ShootAtSpeed.java b/src/org/usfirst/frc/team2059/robot/commands/ShootAtSpeed.java new file mode 100644 index 0000000..713d921 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/ShootAtSpeed.java @@ -0,0 +1,34 @@ +package org.usfirst.frc.team2059.robot.commands; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +import org.usfirst.frc.team2059.robot.RobotMap; +/** + * + */ +public class ShootAtSpeed extends CommandBase { + double speed; + public ShootAtSpeed(double s) { + speed=s; + } + // Called just before this Command runs the first time + protected void initialize() { + } + // Called repeatedly when this Command is scheduled to run + protected void execute() { + shooter.shootAtSpeed(speed); + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + // Called once after isFinished returns true + protected void end() { + shooter.shootAtSpeed(0); + } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/commands/SpinRollers.java b/src/org/usfirst/frc/team2059/robot/commands/SpinRollers.java new file mode 100644 index 0000000..0806efa --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/SpinRollers.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team2059.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import org.usfirst.frc.team2059.robot.Robot; +/** + * + */ +public class SpinRollers extends CommandBase { + double speed; + public SpinRollers(double s) { + speed=s; + } + // 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(SmartDashboard.getBoolean("SmartRollers") && mainArm.getDegrees()<5){ + shooter.shootAtSpeed(-.5); + }else{ + shooter.shootAtSpeed(speed); + } + } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + // Called once after isFinished returns true + protected void end() { + shooter.shootAtSpeed(0); + } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} +// vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java new file mode 100644 index 0000000..8cf6e4e --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java @@ -0,0 +1,15 @@ +package org.usfirst.frc.team2059.robot.subsystems; +import org.usfirst.frc.team2059.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.CANTalon; +public class Shooter extends Subsystem { + CANTalon shooterMotorLeft = new CANTalon(RobotMap.shooterLeftMotor); + CANTalon shooterMotorRight = new CANTalon(RobotMap.shooterRightMotor); + public void initDefaultCommand() { + } + public void shootAtSpeed(double speed){ + shooterMotorRight.set(speed); + shooterMotorLeft.set(speed); + } +} +// vim: sw=2:ts=2:sts=2