From: Kevin Zhang Date: Fri, 13 Nov 2015 01:51:00 +0000 (-0800) Subject: Clean Up Code for FRC 3501-Spark Code: Restructure codebase X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2015-FRC-Spark;a=commitdiff_plain;h=f11ce98e1e3e9741aabfcdc615494b4d6d5630bb Clean Up Code for FRC 3501-Spark Code: Restructure codebase --- f11ce98e1e3e9741aabfcdc615494b4d6d5630bb diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..daf6ebb --- /dev/null +++ b/.classpath @@ -0,0 +1,7 @@ + + + + + + + diff --git a/.project b/.project new file mode 100644 index 0000000..f42f1b9 --- /dev/null +++ b/.project @@ -0,0 +1,18 @@ + + + 2015-FRC-Spark + + + + + + org.eclipse.jdt.core.javabuilder + + + + + + org.eclipse.jdt.core.javanature + edu.wpi.first.wpilib.plugins.core.nature.FRCProjectNature + + diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/OI.class b/bin/org/usfirst/frc3501/RiceCatRobot/OI.class new file mode 100644 index 0000000..2c6d8ad Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/OI.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/Robot.class b/bin/org/usfirst/frc3501/RiceCatRobot/Robot.class new file mode 100644 index 0000000..1de0462 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/Robot.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/RobotMap$Direction.class b/bin/org/usfirst/frc3501/RiceCatRobot/RobotMap$Direction.class new file mode 100644 index 0000000..5d821e1 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/RobotMap$Direction.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/RobotMap.class b/bin/org/usfirst/frc3501/RiceCatRobot/RobotMap.class new file mode 100644 index 0000000..7668172 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/RobotMap.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/CloseClaw.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/CloseClaw.class new file mode 100644 index 0000000..b95fe0c Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/CloseClaw.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.class new file mode 100644 index 0000000..c4bf05d Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmFor.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmFor.class new file mode 100644 index 0000000..a4dbed9 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmFor.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/OpenClaw.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/OpenClaw.class new file mode 100644 index 0000000..9702213 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/OpenClaw.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/ToggleClaw.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/ToggleClaw.class new file mode 100644 index 0000000..743e4e1 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/ToggleClaw.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/ToggleCompressor.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/ToggleCompressor.class new file mode 100644 index 0000000..e3c3c34 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/ToggleCompressor.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/commands/TurnFor.class b/bin/org/usfirst/frc3501/RiceCatRobot/commands/TurnFor.class new file mode 100644 index 0000000..a367133 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/commands/TurnFor.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.class b/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.class new file mode 100644 index 0000000..4695334 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/Claw.class b/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/Claw.class new file mode 100644 index 0000000..99b47d3 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/Claw.class differ diff --git a/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.class b/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.class new file mode 100644 index 0000000..1cc5654 Binary files /dev/null and b/bin/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.class differ diff --git a/build.properties b/build.properties new file mode 100644 index 0000000..6dec74a --- /dev/null +++ b/build.properties @@ -0,0 +1,4 @@ +# Project specific information +package=org.usfirst.frc3501.RiceCatRobot +robot.class=${package}.Robot +simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world \ No newline at end of file diff --git a/build.xml b/build.xml new file mode 100644 index 0000000..2182d37 --- /dev/null +++ b/build.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/org/usfirst/frc3501/RiceCatRobot/OI.java b/src/org/usfirst/frc3501/RiceCatRobot/OI.java new file mode 100644 index 0000000..8477f98 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/OI.java @@ -0,0 +1,30 @@ +package org.usfirst.frc3501.RiceCatRobot; + +import org.usfirst.frc3501.RiceCatRobot.commands.ToggleClaw; +import org.usfirst.frc3501.RiceCatRobot.commands.ToggleCompressor; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.JoystickButton; + +public class OI { + public static Joystick leftJoystick; + public static Joystick rightJoystick; + public static JoystickButton trigger; + public static JoystickButton toggleCompressor; + public static JoystickButton toggleClaw; + + public OI() { + System.out.println("OI is open"); + leftJoystick = new Joystick(RobotMap.LEFT_STICK_PORT); + rightJoystick = new Joystick(RobotMap.RIGHT_STICK_PORT); + + trigger = new JoystickButton(rightJoystick, RobotMap.TRIGGER_PORT); + + toggleClaw = new JoystickButton(rightJoystick, RobotMap.TOGGLE_PORT); + toggleClaw.whenPressed(new ToggleClaw()); + + toggleCompressor = new JoystickButton(rightJoystick, + RobotMap.TOGGLE_COMPRESSOR_PORT); + toggleCompressor.whenPressed(new ToggleCompressor()); + } +} \ No newline at end of file diff --git a/src/org/usfirst/frc3501/RiceCatRobot/Robot.java b/src/org/usfirst/frc3501/RiceCatRobot/Robot.java new file mode 100644 index 0000000..f82f4b2 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/Robot.java @@ -0,0 +1,69 @@ +package org.usfirst.frc3501.RiceCatRobot; + +import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm; +import org.usfirst.frc3501.RiceCatRobot.subsystems.Claw; +import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain; + +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Scheduler; + +public class Robot extends IterativeRobot { + static Timer timer; + public static OI oi; + public static DriveTrain driveTrain; + public static Arm arm; + public static Claw claw; + public static Compressor compressor; + + public void robotInit() { + RobotMap.init(); + driveTrain = new DriveTrain(); + arm = new Arm(); + claw = new Claw(); + oi = new OI(); + compressor = new Compressor(RobotMap.COMPRESSOR_PORT); + } + + public void autonomousInit() { + } + + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + } + + public void teleopInit() { + System.out.println("running teleopInit"); + } + + public void teleopPeriodic() { + Scheduler.getInstance().run(); + + } + + public void operate() { + driveTrain.arcadeDrive(OI.rightJoystick.getY(), + OI.rightJoystick.getTwist()); + claw.doTriggerAction(); + if (OI.leftJoystick.getRawButton(8)) { + arm.setArmSpeeds(0.3); + } else if (OI.leftJoystick.getRawButton(9)) { + arm.setArmSpeeds(-0.3); + } else if (OI.leftJoystick.getRawButton(6)) { + arm.setLeft(0.3); + } else if (OI.leftJoystick.getRawButton(7)) { + arm.setLeft(-0.3); + } else if (OI.leftJoystick.getRawButton(11)) { + arm.setRight(-0.3); + } else if (OI.leftJoystick.getRawButton(10)) { + arm.setRight(0.3); + } + if (Math.abs(OI.leftJoystick.getY()) < 0.05) { + arm.setArmSpeeds(0); + + } else { + arm.fineTuneControl(OI.leftJoystick.getY()); + } + } +} \ No newline at end of file diff --git a/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java b/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java new file mode 100644 index 0000000..4bf6e4a --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java @@ -0,0 +1,41 @@ +package org.usfirst.frc3501.RiceCatRobot; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; + +/** + * 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 { + public final static int LEFT_STICK_PORT = 0, RIGHT_STICK_PORT = 1; + public final static int TRIGGER_PORT = 1, TOGGLE_PORT = 2, + TOGGLE_COMPRESSOR_PORT = 11; + public static final int DRIVE_FRONT_LEFT = 4, DRIVE_FRONT_RIGHT = 5, + DRIVE_REAR_LEFT = 3, DRIVE_REAR_RIGHT = 6; + public static final int DRIVE_LEFT_A = 3, DRIVE_LEFT_B = 4, + DRIVE_RIGHT_A = 2, DRIVE_RIGHT_B = 1; + + public static final double DISTANCE_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256; + + public static final int ARM_LEFT = 2, ARM_RIGHT = 7; + public static final double ARM_HIGH_SPEED = 0.5, ARM_LOW_SPEED = 0.5; + + // Claw + public static final int SOLENOID_FORWARD = 0, SOLENOID_REVERSE = 1, + MODULE_NUMBER = 0; + public final static Value open = DoubleSolenoid.Value.kForward, + close = DoubleSolenoid.Value.kReverse; + public static double DRIVE_DEAD_ZONE = 0.25; + // Compressor + public static final int COMPRESSOR_PORT = 0; + + public static void init() { + } + + public static enum Direction { + LEFT, RIGHT, DOWN, UP, FORWARD, BACKWARD; + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/CloseClaw.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/CloseClaw.java new file mode 100644 index 0000000..eb0eea2 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/CloseClaw.java @@ -0,0 +1,36 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import edu.wpi.first.wpilibj.command.Command; + +import org.usfirst.frc3501.RiceCatRobot.Robot; + +/** + * + */ +public class CloseClaw extends Command { + + public CloseClaw() { + requires(Robot.claw); + } + + protected void initialize() { + System.out.println("IN INIT CLOSECLAW"); + } + + protected void execute() { + Robot.claw.closeClaw(); + System.out.println("Closing claw"); + } + + protected boolean isFinished() { + System.out.println("Claw Closed"); + return !Robot.claw.isOpen(); + } + + protected void end() { + + } + + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.java new file mode 100644 index 0000000..b935311 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.java @@ -0,0 +1,77 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.Robot; +import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +/** + * This command takes a time in seconds which is how long it should run + * + */ +public class DriveFor extends Command { + private double seconds; + private Timer timer; + private Direction direction; + + public DriveFor(double seconds, Direction direction) { + this.seconds = seconds; + this.direction = direction; + + } + + @Override + protected void initialize() { + timer = new Timer(); + timer.reset(); + timer.start(); + } + + @Override + protected void execute() { + System.out.println(timer.get()); + if (direction == Direction.FORWARD) { + if (timer.get() < seconds * 0.2) { // for the first 20% of time, run + // the robot at -.5 speed + Robot.driveTrain.arcadeDrive(-0.3, 0); + } else if (timer.get() >= seconds * 0.2 + && timer.get() <= seconds * 0.8) { // for the +20% - 75% + // time, move the robot + // at -.3 speed. + Robot.driveTrain.arcadeDrive(-0.5, 0); + } else if (timer.get() < seconds) { + Robot.driveTrain.arcadeDrive(-0.25, 0); + } else { + Robot.driveTrain.arcadeDrive(0, 0); + } + } else if (direction == Direction.BACKWARD) { + if (timer.get() < seconds * 0.2) { + Robot.driveTrain.arcadeDrive(0.3, 0); + } else if (timer.get() >= seconds * 0.2 + && timer.get() <= seconds * 0.8) { + Robot.driveTrain.arcadeDrive(0.5, 0); + } else if (timer.get() < seconds) { + Robot.driveTrain.arcadeDrive(0.25, 0); + } else { + Robot.driveTrain.arcadeDrive(0, 0); + } + } + } + + @Override + protected boolean isFinished() { + return timer.get() > seconds; + } + + @Override + protected void end() { + Robot.driveTrain.arcadeDrive(0, 0); + } + + @Override + protected void interrupted() { + end(); + } + +} \ No newline at end of file diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmFor.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmFor.java new file mode 100644 index 0000000..e94f859 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmFor.java @@ -0,0 +1,61 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.Robot; +import org.usfirst.frc3501.RiceCatRobot.RobotMap; +import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +/** + * This command takes a time in seconds which is how long it should run to move + * arm up or down + * + */ +public class MoveArmFor extends Command { + private double seconds; + private Timer timer; + private Direction direction; + + /* + * @param Direction must be up or down + */ + public MoveArmFor(double seconds, Direction direction) { + this.seconds = seconds; + this.direction = direction; + } + + @Override + protected void initialize() { + timer = new Timer(); + timer.start(); + } + + @Override + protected void execute() { + if (direction == Direction.UP) { + Robot.arm.setArmSpeeds(-RobotMap.ARM_LOW_SPEED); + } else if (direction == Direction.DOWN) { + Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED); + } + } + + @Override + protected boolean isFinished() { + if (timer.get() > seconds) { + Robot.arm.setArmSpeeds(0); + } + return timer.get() > seconds; + } + + @Override + protected void end() { + Robot.arm.setArmSpeeds(0); + } + + @Override + protected void interrupted() { + end(); + } + +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/OpenClaw.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/OpenClaw.java new file mode 100644 index 0000000..bece53b --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/OpenClaw.java @@ -0,0 +1,37 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import edu.wpi.first.wpilibj.command.Command; + +import org.usfirst.frc3501.RiceCatRobot.Robot; + +/** + * Opens claw by reversing solenoids. + * + */ +public class OpenClaw extends Command { + + public OpenClaw() { + requires(Robot.claw); + } + + protected void initialize() { + System.out.println("IN INIT OPENCLAW"); + } + + protected void execute() { + Robot.claw.openClaw(); + System.out.println("Opening Claw"); + } + + protected boolean isFinished() { + System.out.println("Claw Opened"); + return Robot.claw.isOpen(); + } + + protected void end() { + + } + + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/ToggleClaw.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/ToggleClaw.java new file mode 100644 index 0000000..f746056 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/ToggleClaw.java @@ -0,0 +1,36 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ToggleClaw extends Command { + + public ToggleClaw() { + + } + + protected void initialize() { + Robot.claw.toggleOn = !Robot.claw.toggleOn; + } + + protected void execute() { + if (Robot.claw.toggleOn && Robot.claw.isOpen()) { + Robot.claw.closeClaw(); + } + } + + protected boolean isFinished() { + return true; + } + + protected void end() { + + } + + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/ToggleCompressor.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/ToggleCompressor.java new file mode 100644 index 0000000..5f3fad1 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/ToggleCompressor.java @@ -0,0 +1,43 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ToggleCompressor extends Command { + + public ToggleCompressor() { + } + + // 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.compressor.enabled()) { + Robot.compressor.stop(); + } else { + Robot.compressor.start(); + } + + } + + // 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 + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/TurnFor.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/TurnFor.java new file mode 100644 index 0000000..9be3779 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/TurnFor.java @@ -0,0 +1,54 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.Robot; +import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +public class TurnFor extends Command { + private double seconds; + private Timer timer; + private Direction direction; + + public TurnFor(double seconds, Direction direction) { + this.seconds = seconds; + this.direction = direction; + } + + @Override + protected void initialize() { + timer = new Timer(); + timer.start(); + } + + @Override + protected void execute() { + if (direction == Direction.LEFT) { + Robot.driveTrain.arcadeDrive(0, -0.5); + } else if (direction == Direction.RIGHT) { + Robot.driveTrain.arcadeDrive(0, 0.5); + } else { + Robot.driveTrain.arcadeDrive(0, 0); + } + } + + @Override + protected boolean isFinished() { + System.out.println(timer.get()); + System.out.println(seconds); + if (timer.get() > seconds) { + Robot.driveTrain.arcadeDrive(0, 0); + } + return timer.get() > seconds; + } + + @Override + protected void end() { + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java new file mode 100644 index 0000000..2ba6b5b --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java @@ -0,0 +1,48 @@ +package org.usfirst.frc3501.RiceCatRobot.subsystems; + +import org.usfirst.frc3501.RiceCatRobot.RobotMap; + +import edu.wpi.first.wpilibj.CANJaguar; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Arm extends Subsystem { + private CANJaguar left, right; + + public Arm() { + left = new CANJaguar(RobotMap.ARM_LEFT); + right = new CANJaguar(RobotMap.ARM_RIGHT); + } + + public void initDefaultCommand() { + } + + public void fineTuneControl(double d) { + if (Math.abs(d) < 0.05) { + d = 0; + } else if (d > 0) { + d *= d; + } else { + d *= -d; + } + setArmSpeeds(d); + } + + public void setLeft(double speed) { + left.set(-speed); + } + + public void setRight(double speed) { + right.set(-speed); + } + + public void setArmSpeeds(double speed) { + setLeft(speed); + setRight(speed); + } + + public void stop() { + left.set(0); + right.set(0); + } + +} \ No newline at end of file diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Claw.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Claw.java new file mode 100644 index 0000000..100eff4 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Claw.java @@ -0,0 +1,50 @@ +package org.usfirst.frc3501.RiceCatRobot.subsystems; + +import org.usfirst.frc3501.RiceCatRobot.OI; +import org.usfirst.frc3501.RiceCatRobot.Robot; +import org.usfirst.frc3501.RiceCatRobot.RobotMap; +import org.usfirst.frc3501.RiceCatRobot.commands.CloseClaw; +import org.usfirst.frc3501.RiceCatRobot.commands.OpenClaw; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Claw extends Subsystem { + + private DoubleSolenoid solenoid; + public boolean toggleOn = false; + + public Claw() { + solenoid = new DoubleSolenoid(RobotMap.MODULE_NUMBER, + RobotMap.SOLENOID_FORWARD, RobotMap.SOLENOID_REVERSE); + } + + public void initDefaultCommand() { + } + + public void closeClaw() { + solenoid.set(RobotMap.close); + } + + public void openClaw() { + solenoid.set(RobotMap.open); + } + + public boolean isOpen() { + return solenoid.get() == RobotMap.open; + } + + public void doTriggerAction() { + if (!Robot.claw.toggleOn) { + if (OI.rightJoystick.getRawButton(RobotMap.TRIGGER_PORT)) { + if (Robot.claw.isOpen()) { + new CloseClaw().start(); + } + } else { + if (!Robot.claw.isOpen()) { + new OpenClaw().start(); + } + } + } + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java new file mode 100644 index 0000000..9307c78 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/DriveTrain.java @@ -0,0 +1,107 @@ +package org.usfirst.frc3501.RiceCatRobot.subsystems; + +import org.usfirst.frc3501.RiceCatRobot.RobotMap; + +import edu.wpi.first.wpilibj.CANJaguar; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.CounterBase.EncodingType; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class DriveTrain extends Subsystem { + private CANJaguar frontLeft, frontRight, rearLeft, rearRight; + private Encoder leftEncoder, rightEncoder; + + public DriveTrain() { + frontLeft = new CANJaguar(RobotMap.DRIVE_FRONT_LEFT); + frontRight = new CANJaguar(RobotMap.DRIVE_FRONT_RIGHT); + rearLeft = new CANJaguar(RobotMap.DRIVE_REAR_LEFT); + rearRight = new CANJaguar(RobotMap.DRIVE_REAR_RIGHT); + leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_A, RobotMap.DRIVE_LEFT_B, + false, EncodingType.k4X); + rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_A, + RobotMap.DRIVE_RIGHT_B, false, EncodingType.k4X); + leftEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE); + rightEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE); + } + + public void resetEncoders() { + leftEncoder.reset(); + rightEncoder.reset(); + } + + public double getRightSpeed() { + // Returns in per second + return rightEncoder.getRate(); + } + + public double getLeftSpeed() { + return leftEncoder.getRate(); + } + + public double getRightDistance() { + // Returns distance in in + return rightEncoder.getDistance(); + } + + public double getLeftDistance() { + // Returns distance in in + return leftEncoder.getDistance(); + } + + public void setMotorSpeeds(double leftSpeed, double rightSpeed) { + if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) { + leftSpeed = 0; + } + if (Math.abs(rightSpeed) < RobotMap.DRIVE_DEAD_ZONE) { + rightSpeed = 0; + } + this.frontLeft.set(leftSpeed); + this.frontRight.set(-rightSpeed); + this.rearLeft.set(leftSpeed); + this.rearRight.set(-rightSpeed); + } + + public void arcadeDrive(double yVal, double twist) { + if (yVal < 0 && Math.abs(yVal) < 0.1125) { + yVal = 0; + } else if (yVal > 0 && Math.abs(yVal) < 0.25) { + yVal = 0; + } else if (yVal > 0) { + yVal -= 0.25; + } else if (yVal < 0) { + yVal += 0.15; + } + if (Math.abs(twist) < RobotMap.DRIVE_DEAD_ZONE) { + twist = 0; + } + + double leftMotorSpeed; + double rightMotorSpeed; + // adjust the sensitivity (yVal+rootof (yval)) / 2 + yVal = (yVal + Math.signum(yVal) * Math.sqrt(Math.abs(yVal))) / 2; + // adjust the sensitivity (twist+rootof (twist)) / 2 + twist = (twist + Math.signum(twist) * Math.sqrt(Math.abs(twist))) / 2; + if (yVal > 0) { + if (twist > 0) { + leftMotorSpeed = yVal - twist; + rightMotorSpeed = Math.max(yVal, twist); + } else { + leftMotorSpeed = Math.max(yVal, -twist); + rightMotorSpeed = yVal + twist; + } + } else { + if (twist > 0.0) { + leftMotorSpeed = -Math.max(-yVal, twist); + rightMotorSpeed = yVal + twist; + } else { + leftMotorSpeed = yVal - twist; + rightMotorSpeed = -Math.max(-yVal, -twist); + } + } + setMotorSpeeds(leftMotorSpeed, rightMotorSpeed); + } + + @Override + protected void initDefaultCommand() { + } +}