public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256;
public static double kp = 0.013, ki = 0.000015, kd = -0.002;
- public static double gp = 0.018, gi = 0.000015, gd = 0;
- public static double encoderTolerance = 8.0, gyroTolerance = 5.0;
-
- public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
- public static double time = 0;
+ public static double encoderTolerance = 8.0;
// Gearing constants
public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
- public static boolean inverted = false;
-
public static final double PASS_DEFENSE_TIMEOUT = 10; // find this
}
public static final double INTAKE_SPEED = 0.5;
public static final double OUTPUT_SPEED = -0.5;
}
-
- public static class DefenseArm {
- // Potentiometer related ports
- public static final int ARM_CHANNEL = 0;
- public static final int ARM_PORT = 0;
- public static final int HAND_PORT = 1;
- public static final int HAND_CHANNEL = 1;
- public final static double FULL_RANGE = 270.0; // in degrees
- public final static double OFFSET = -135.0; // in degrees
-
- public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3
- // level
- public final static double ARM_LENGTH = 0; // TODO: find actual length
- public final static double HAND_LENGTH = 0; // TODO: find actual length
- public final static double ARM_MOUNTED_HEIGHT = 0; // TODO: find actual
- // height
- }
-
- public static class Auton {
- /*
- * Distance dead reckoning constants
- */
- public static final double POS1_DIST1 = 109;
- public static final double POS1_TURN1 = 60;
- public static final double POS1_DIST2 = 0;
-
- // constants for position 2
- public static final double POS2_DIST1 = 140;
- public static final double POS2_TURN1 = 60;
- public static final double POS2_DIST2 = 0;
-
- // constants for position 3
- public static final double POS3_DIST1 = 0;
- public static final double POS3_TURN1 = 90;
- public static final double POS3_DIST2 = 35.5;
- public static final double POS3_TURN2 = -90;
- public static final double POS3_DIST3 = 0;
-
- // constants for position 4
- public static final double POS4_DIST1 = 0;
- public static final double POS4_TURN1 = -90;
- public static final double POS4_DIST2 = 18.5;
- public static final double POS4_TURN2 = 90;
- public static final double POS4_DIST3 = 0;
-
- // constants for position 5
- public static final double POS5_DIST1 = 0;
- public static final double POS5_TURN1 = -90;
- public static final double POS5_DIST2 = 72.5;
- public static final double POS5_TURN2 = 90;
- public static final double POS5_DIST3 = 0;
- public static final double DRIVE_MAX_TIMEOUT = 3.0;
- public static final double TURN_MAX_TIMEOUT = 5.0;
-
- /*
- * Time dead Reckoning constants
- */
- public static final double POS1_DIST1_TIME = 109;
- public static final double POS1_DRIVE_MAXSPEED = 0.5;
- public static final double POS1_TURN1_TIME = 60;
- public static final double POS1_TURN_MAXSPEED = 0.5;
- public static final double POS1_DIST2_TIME = 0;
-
- // constants for position 2
-
- public static final double POS2_DIST1_TIME = 109;
- public static final double POS2_DRIVE_MAXSPEED = 0.5;
- public static final double POS2_TURN1_TIME = 60;
- public static final double POS2_TURN_MAXSPEED = 0.5;
- public static final double POS2_DIST2_TIME = 0;
-
- // constants for position 3
-
- public static final double POS3_DIST1_TIME = 109;
- public static final double POS3_DRIVE_MAXSPEED = 0.5;
- public static final double POS3_TURN1_TIME = 60;
- public static final double POS3_TURN_MAXSPEED = 0.5;
- public static final double POS3_DIST2_TIME = 0;
- // constants for position 4
-
- public static final double POS4_DIST1_TIME = 109;
- public static final double POS4_DRIVE_MAXSPEED = 0.5;
- public static final double POS4_TURN1_TIME = 60;
- public static final double POS4_TURN_MAXSPEED = 0.5;
- public static final double POS4_DIST2_TIME = 0;
- // constants for position 5
-
- public static final double POS5_DIST1_TIME = 109;
- public static final double POS5_DRIVE_MAXSPEED = 0.5;
- public static final double POS5_TURN1_TIME = 60;
- public static final double POS5_TURN_MAXSPEED = 0.5;
- public static final double POS5_DIST2_TIME = 0;
-
- // Passing Defenses Constants
-
- public static final double DEFAULT_SPEED = 0.5;
- public static final boolean IS_USING_TIME = true;
-
- // dead reckoning time and speed constants for driving through defenses
- // TODO: find the times it takes to pass each defense
- public static final double PASS_ROCK_WALL_TIME = 0;
- public static final double PASS_ROCK_WALL_SPEED = 0;
- public static final double PASS_ROCK_WALL_DIST = 0;
- public static final double PASS_LOW_BAR_TIME = 0;
- public static final double PASS_LOW_BAR_SPEED = 0;
- public static final double PASS_LOW_BAR_DIST = 0;
- public static final double PASS_MOAT_TIME = 0;
- public static final double PASS_MOAT_SPEED = 0;
- public static final double PASS_MOAT_DIST = 0;
- public static final double PASS_RAMPART_TIME = 0;
- public static final double PASS_RAMPART_SPEED = 0;
- public static final double PASS_RAMPART_DIST = 0;
- public static final double PASS_ROUGH_TERRAIN_TIME = 0;
- public static final double PASS_ROUGH_TERRAIN_SPEED = 0;
- public static final double PASS_ROUGH_TERRAIN_DIST = 0;
- }
-
- public enum Direction {
- UP, DOWN, RIGHT, LEFT, FORWARD, BACKWARD;
- }
-
- public enum Defense {
- PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHEVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART;
- }
}
import org.usfirst.frc.team3501.robot.commands.intakearm.RunIntake;
import org.usfirst.frc.team3501.robot.commands.intakearm.StopIntake;
import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
-import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
-
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
package org.usfirst.frc.team3501.robot;
-import org.usfirst.frc.team3501.robot.Constants.Defense;
import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
+import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm;
import org.usfirst.frc.team3501.robot.commands.shooter.ResetCatapult;
-import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
-import org.usfirst.frc.team3501.robot.subsystems.Scaler;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Robot extends IterativeRobot {
public static OI oi;
public static DriveTrain driveTrain;
public static Shooter shooter;
-
- public static Scaler scaler;
-
public static IntakeArm intakeArm;
- public static DefenseArm defenseArm;
-
- // Sendable Choosers send a drop down menu to the Smart Dashboard.
- SendableChooser positionChooser;
- SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
- positionFourDefense, positionFiveDefense;
@Override
public void robotInit() {
oi = new OI();
shooter = new Shooter();
- scaler = new Scaler();
intakeArm = new IntakeArm();
-
- initializeSendableChoosers();
- addPositionChooserOptions();
- addDefensesToAllDefenseSendableChoosers();
- sendSendableChoosersToSmartDashboard();
-
- }
-
- private void initializeSendableChoosers() {
- positionChooser = new SendableChooser();
- positionOneDefense = new SendableChooser();
- positionTwoDefense = new SendableChooser();
- positionThreeDefense = new SendableChooser();
- positionFourDefense = new SendableChooser();
- positionFiveDefense = new SendableChooser();
- }
-
- private void addPositionChooserOptions() {
- positionChooser.addDefault("Position 1", 1);
- positionChooser.addObject("Position 2", 2);
- positionChooser.addObject("Position 3", 3);
- positionChooser.addObject("Position 4", 4);
- positionChooser.addObject("Position 5", 5);
- }
-
- private void addDefensesToAllDefenseSendableChoosers() {
- addDefenseOptions(positionOneDefense);
- addDefenseOptions(positionTwoDefense);
- addDefenseOptions(positionThreeDefense);
- addDefenseOptions(positionFourDefense);
- addDefenseOptions(positionFiveDefense);
- }
-
- private void addDefenseOptions(SendableChooser chooser) {
- chooser.addDefault("Portcullis", Defense.PORTCULLIS);
- chooser.addObject("Sally Port", Defense.SALLY_PORT);
- chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
- chooser.addObject("Low Bar", Defense.LOW_BAR);
- chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
- chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
- chooser.addObject("Moat", Defense.MOAT);
- chooser.addObject("Rock Wall", Defense.ROCK_WALL);
- }
-
- private void sendSendableChoosersToSmartDashboard() {
- SmartDashboard.putData("PositionChooser", positionChooser);
- SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
- SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
- SmartDashboard.putData("Position Three Defense Chooser",
- positionThreeDefense);
- SmartDashboard
- .putData("Position Four Defense Chooser", positionFourDefense);
- SmartDashboard
- .putData("Position Five Defense Chooser", positionFiveDefense);
-
- SmartDashboard
- .putData("Position Four Defense Chooser", positionFourDefense);
- SmartDashboard
- .putData("Position Five Defense Chooser", positionFiveDefense);
-
- shooter = new Shooter();
-
}
@Override
public void autonomousInit() {
Scheduler.getInstance().run();
-
- // get options chosen from drop down menu
- Integer chosenPosition = (Integer) positionChooser.getSelected();
- Integer chosenDefense = 0;
-
- if (chosenPosition == 1)
- chosenDefense = (Integer) positionOneDefense.getSelected();
- else if (chosenPosition == 2)
- chosenDefense = (Integer) positionTwoDefense.getSelected();
- else if (chosenPosition == 3)
- chosenDefense = (Integer) positionThreeDefense.getSelected();
- else if (chosenPosition == 4)
- chosenDefense = (Integer) positionFourDefense.getSelected();
- else if (chosenPosition == 5)
- chosenDefense = (Integer) positionFiveDefense.getSelected();
-
- System.out.println("Chosen Position: " + chosenPosition);
- System.out.println("Chosen Defense: " + chosenDefense);
}
@Override
// gear
Scheduler.getInstance().add(new ResetCatapult()); // Reset catapult at start
// of each match.
+
+ Scheduler.getInstance().add(new MoveIntakeArm(Constants.IntakeArm.EXTEND));
+ // Start testing with intake arm extended TODO remove this
}
@Override
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class AimAndAlign extends Command {
-
- public AimAndAlign() {
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime;
-import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- * This command group will be used in autonomous. Based on what position the
- * robot is in, the robot will align with the goal. In the Software 2015-2016
- * Google folder is a picture explaining each of the cases.
- *
- * dependency on sensors: lidars, encoders, gyro
- *
- * dependency on subsystems: drivetrain
- *
- * dependency on other commands: TurnForAngle(), DriveForDistance()
- *
- * pre-condition: robot is flush against a defense at the specified position in
- * the opponent's courtyard
- *
- * post-condition: the robot is parallel to one of the three goals and the
- * shooter is facing that goal
- *
- */
-public class AlignToScore extends CommandGroup {
-
- private final double DEFAULT_SPEED = 0.5;
- private final double maxTimeout = 5;
-
- // in inches
- // assuming that positive angle means turning right
- // and negative angle means turning left
-
- // constants for position 1: low bar
-
- public double horizontalDistToGoal;
-
- public AlignToScore(int position) {
- if (Constants.Auton.IS_USING_TIME) {
- if (position == 1) {
- addSequential(new DriveForTime(Constants.Auton.POS1_DIST1_TIME,
- Constants.Auton.POS1_DRIVE_MAXSPEED));
- addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1,
- Constants.Auton.POS1_TURN_MAXSPEED));
- addSequential(new DriveDistance(Constants.Auton.POS1_DIST2_TIME,
- Constants.Auton.POS1_DRIVE_MAXSPEED));
- horizontalDistToGoal = 0;
- }
- else if (position == 2) {
- addSequential(new DriveForTime(Constants.Auton.POS2_DIST1_TIME,
- Constants.Auton.POS2_DRIVE_MAXSPEED));
- addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1,
- Constants.Auton.POS2_TURN_MAXSPEED));
- addSequential(new DriveDistance(Constants.Auton.POS2_DIST2_TIME,
- Constants.Auton.POS2_DRIVE_MAXSPEED));
- horizontalDistToGoal = 0;
- }
- else if (position == 3) {
- addSequential(new DriveForTime(Constants.Auton.POS3_DIST1_TIME,
- Constants.Auton.POS3_DRIVE_MAXSPEED));
- addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1,
- Constants.Auton.POS3_TURN_MAXSPEED));
- addSequential(new DriveDistance(Constants.Auton.POS3_DIST2_TIME,
- Constants.Auton.POS3_DRIVE_MAXSPEED));
- horizontalDistToGoal = 0;
- }
- else if (position == 4) {
- addSequential(new DriveForTime(Constants.Auton.POS4_DIST1_TIME,
- Constants.Auton.POS4_DRIVE_MAXSPEED));
- addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1,
- Constants.Auton.POS4_TURN_MAXSPEED));
- addSequential(new DriveDistance(Constants.Auton.POS4_DIST2_TIME,
- Constants.Auton.POS4_DRIVE_MAXSPEED));
- horizontalDistToGoal = 0;
- }
- else if (position == 5) {
- addSequential(new DriveForTime(Constants.Auton.POS5_DIST1_TIME,
- Constants.Auton.POS5_DRIVE_MAXSPEED));
- addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1,
- Constants.Auton.POS5_TURN_MAXSPEED));
- addSequential(new DriveDistance(Constants.Auton.POS5_DIST2_TIME,
- Constants.Auton.POS5_DRIVE_MAXSPEED));
- horizontalDistToGoal = 0;
- }
- }
- else {
- // position 1 is always the low bar
- if (position == 1) {
- addSequential(new DriveDistance(Constants.Auton.POS1_DIST1,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS1_TURN1,
- Constants.Auton.TURN_MAX_TIMEOUT));
- addSequential(new DriveDistance(Constants.Auton.POS1_DIST2,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- } else if (position == 2) {
-
- addSequential(new DriveDistance(Constants.Auton.POS2_DIST1,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS2_TURN1,
- Constants.Auton.TURN_MAX_TIMEOUT));
- addSequential(new DriveDistance(Constants.Auton.POS2_DIST2,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
-
- } else if (position == 3) {
-
- addSequential(new DriveDistance(Constants.Auton.POS3_DIST1,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS3_TURN1, maxTimeout));
- addSequential(new DriveDistance(Constants.Auton.POS3_DIST2,
- Constants.Auton.TURN_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS3_TURN2, maxTimeout));
- addSequential(new DriveDistance(Constants.Auton.POS3_DIST3,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
-
- } else if (position == 4) {
-
- addSequential(new DriveDistance(Constants.Auton.POS4_DIST1,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS4_TURN1, maxTimeout));
- addSequential(new DriveDistance(Constants.Auton.POS4_DIST2,
- Constants.Auton.TURN_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS4_TURN2, maxTimeout));
- addSequential(new DriveDistance(Constants.Auton.POS4_DIST3,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
-
- } else if (position == 5) {
-
- addSequential(new DriveDistance(Constants.Auton.POS5_DIST1,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS5_TURN1, maxTimeout));
- addSequential(new DriveDistance(Constants.Auton.POS5_DIST2,
- Constants.Auton.TURN_MAX_TIMEOUT));
- addSequential(new TurnForAngle(Constants.Auton.POS5_TURN2, maxTimeout));
- addSequential(new DriveDistance(Constants.Auton.POS5_DIST3,
- Constants.Auton.DRIVE_MAX_TIMEOUT));
- }
- }
- }
- // following commented out method is calculations for path of robot in auton
- // after passing through defense using two lidars
- /*
- * public static double lidarCalculateAngleToTurn(int position,
- * double horizontalDistToGoal) {
- * double leftDist = Robot.driveTrain.getLeftLidarDistance();
- * double rightDist = Robot.driveTrain.getRightLidarDistance();
- *
- * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2);
- * double distToTower;
- * // TODO: figure out if we do want to shoot into the side goal if we are
- * // in position 1 or 2, or if we want to change that
- * if (position == 1 || position == 2) {
- * distToTower = Math
- * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
- * - DIST_CASTLE_WALL_TO_SIDE_GOAL;
- * }
- *
- * // TODO: figure out if we do want to shoot into the font goal if we are
- * // in position 3, 4, 5, or if we want to change that
- * else {
- * distToTower = Math
- * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
- * - DIST_CASTLE_WALL_TO_SIDE_GOAL;
- * }
- *
- * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal);
- *
- * return angleToTurn;
- * }
- */
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- *
- */
-public class CompactRobot extends CommandGroup {
-
- // TODO: implement CompactRobot with updated Robot capabilities
- public CompactRobot() {
-
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Constants.Defense;
-import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-import edu.wpi.first.wpilibj.command.WaitCommand;
-
-/**
- * The default autonomous strategy involves passing the defense that is in front
- * of it, aiming the robot/ shooter towards the goal, and shooting.
- */
-
-public class DefaultAutonStrategy extends CommandGroup {
-
- public DefaultAutonStrategy(int position, Defense defense) {
-
- if (defense == Constants.Defense.PORTCULLIS)
- addSequential(new LiftPortcullis());
-
- else if (defense == Constants.Defense.SALLY_PORT)
- addSequential(new PassSallyPort());
-
- else if (defense == Constants.Defense.ROUGH_TERRAIN)
- addSequential(new PassRoughTerrain());
-
- else if (defense == Constants.Defense.LOW_BAR)
- addSequential(new PassLowBar());
-
- else if (defense == Constants.Defense.CHEVAL_DE_FRISE)
- addSequential(new PassChevalDeFrise());
-
- else if (defense == Constants.Defense.DRAWBRIDGE)
- addSequential(new PassDrawBridge());
-
- else if (defense == Constants.Defense.MOAT)
- addSequential(new PassMoat());
-
- else if (defense == Constants.Defense.ROCK_WALL)
- addSequential(new PassRockWall());
-
- else if (defense == Constants.Defense.RAMPART)
- addSequential(new PassRampart());
-
- addSequential(new AlignToScore(position));
- // TODO: test for how long robot should wait
- addSequential(new WaitCommand(5.0));
- addSequential(new Shoot());
-
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will move the robot forward while lifting the defense arm up.
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the portcullis
- *
- * This command has to drive forward at the same time because that's how the
- * robot opens the portcullis
- *
- * post-condition: the robot has passed the portcullis and is in the next zone
- *
- * @author Meryem and Avi
- *
- */
-
-public class LiftPortcullis extends Command {
-
- public LiftPortcullis() {
- }
-
- @Override
- protected void end() {
-
- }
-
- @Override
- protected void execute() {
-
- }
-
- @Override
- protected void initialize() {
-
- }
-
- @Override
- protected void interrupted() {
-
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.commands.scaler.RunWinchContinuous;
-import org.usfirst.frc.team3501.robot.commands.scaler.StopWinch;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command group runs the winch to lift robot.
- * Requires Scaler
- *
- * @author shaina
- *
- */
-public class LiftRobot extends CommandGroup {
-
- public LiftRobot() {
- requires(Robot.scaler);
-
- addSequential(new RunWinchContinuous(Constants.Scaler.SCALE_SPEED, Constants.Scaler.SECONDS_TO_SCALE));
- addSequential(new StopWinch());
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will move the intake arm downwards using a motor to lower the
- * cheval de frise and drive over it.
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the cheval de frise
- *
- * post-condition: the robot has passed the cheval de frise and is in the next
- * zone
- *
- * @author Meryem and Avi
- *
- */
-public class PassChevalDeFrise extends Command {
-
- public PassChevalDeFrise() {
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void interrupted() {
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will lower the draw bridge and go through it
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the draw bridge
- *
- * post-condition: the robot has passed the draw bridge and is in the next zone
- *
- * @author Meryem and Avi
- *
- */
-public class PassDrawBridge extends Command {
-
- public PassDrawBridge() {
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void interrupted() {
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command will drive the robot through the low bar.
- *
- * dependency on sensors: encoders
- * dependency on subsystems: drivetrain
- * dependency on other commands: DriveForDist
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the low bar
- *
- * post-condition: the robot has passed the low bar and is in the next zone
- *
- * @author Meryem and Avi
- *
- */
-
-public class PassLowBar extends CommandGroup {
-
- public PassLowBar() {
- if (Constants.Auton.IS_USING_TIME) {
- addSequential(new DriveForTime(Constants.Auton.PASS_LOW_BAR_TIME,
- Constants.Auton.PASS_LOW_BAR_SPEED));
- }
- else {
- addSequential(new DriveDistance(
- Constants.Auton.PASS_LOW_BAR_DIST,
- Constants.DriveTrain.PASS_DEFENSE_TIMEOUT));
- }
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command will drive the robot through the moat.
- *
- * The code drives the robot for a specific time at a specific speed up the ramp
- * to the defense then drive over the defense at a different speed and time.
- *
- * dependency on subsystem: drivetrain
- *
- * dependency on other commands: DriveForTime
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the moat
- *
- * post-condition: the robot has passed the moat and is in the next zone
- *
- * @author Meryem and Avi
- *
- */
-
-public class PassMoat extends CommandGroup {
-
- public PassMoat() {
- if (Constants.Auton.IS_USING_TIME) {
- addSequential(new DriveForTime(Constants.Auton.PASS_MOAT_TIME,
- Constants.Auton.PASS_MOAT_SPEED));
- }
- else {
- addSequential(new DriveDistance(Constants.Auton.PASS_MOAT_DIST,
- Constants.DriveTrain.PASS_DEFENSE_TIMEOUT));
- }
-
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- *
- */
-public class PassPortcullis extends CommandGroup {
-
- public PassPortcullis() {
- // TODO: in theory, these two commands should not be running in parallel all
- // the time, because of specifics of the series of actions needed to
- // successfully pass the portcullis, so edit these to reflect that
- addParallel(new LiftPortcullis());
- addParallel(new DriveDistance(0, 0)); // TODO: figure out distance to travel
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command will drive the robot through the rampart.
- *
- * dependency on subsystems: drivetrain
- *
- * dependency on other commands: DriveForTime
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the rampart
- *
- * post-condition: the robot has passed the rampart and is in the next zone
- *
- * @author Meryem and Avi
- *
- */
-
-public class PassRampart extends CommandGroup {
-
- public PassRampart() {
-
- if (Constants.Auton.IS_USING_TIME) {
- addSequential(new DriveForTime(Constants.Auton.PASS_RAMPART_TIME,
- Constants.Auton.PASS_RAMPART_SPEED));
- }
- else {
- addSequential(new DriveDistance(
- Constants.Auton.PASS_RAMPART_DIST,
- Constants.DriveTrain.PASS_DEFENSE_TIMEOUT));
- }
-
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command will drive the robot through the rock wall.
- *
- * dependency on subsystems: drivetrain
- *
- * dependency on other commands: DriveForTime
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the rock wall
- *
- * post-condition: the robot has passed the rock wall and is in the next zone
- *
- * @author Meryem and Avi
- *
- */
-
-public class PassRockWall extends CommandGroup {
-
- public PassRockWall() {
- if (Constants.Auton.IS_USING_TIME) {
- addSequential(new DriveForTime(Constants.Auton.PASS_ROCK_WALL_TIME,
- Constants.Auton.PASS_ROCK_WALL_SPEED));
- }
- else {
- addSequential(new DriveDistance(
- Constants.Auton.PASS_ROCK_WALL_DIST,
- Constants.DriveTrain.PASS_DEFENSE_TIMEOUT));
- }
-
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command will drive the robot through the rough terrain.
- *
- * dependency on subsytems: drivetrain
- *
- * dependency on other commands: DriveForTime
- *
- * pre-condition: robot is flush against the ramp of the outerworks in front of
- * the rough terrain
- *
- * post-condition: the robot has passed the rough terrain and is in the next
- * zone
- *
- * @author Meryem and Avi
- *
- */
-public class PassRoughTerrain extends CommandGroup {
-
- public PassRoughTerrain() {
-
- if (Constants.Auton.IS_USING_TIME) {
- addSequential(new DriveForTime(Constants.Auton.PASS_ROUGH_TERRAIN_TIME,
- Constants.Auton.PASS_ROUGH_TERRAIN_SPEED));
- }
- else {
- addSequential(new DriveDistance(
- Constants.Auton.PASS_ROUGH_TERRAIN_DIST,
- Constants.DriveTrain.PASS_DEFENSE_TIMEOUT));
- }
-
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.auton;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class PassSallyPort extends Command {
- /***
- * This command will only open the sally port pass through it. It will do this
- * by hooking onto the port and driving backwards while rotating then driving
- * backwards through the sally port.
- *
- * pre-condition: robot is in the neutral zone, flush against the ramp of the
- * outerworks in front of the portcullis
- *
- * post-condition: the robot has passed the sally port and is in a courtyard
- *
- * note: to go from the courtyard to neutral zone, the driver just has to
- * drive through the port
- *
- * @author Meryem and Avi
- *
- */
-
- public PassSallyPort() {
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command is intended to be run from OI using button.whileHeld(...).
- * It lowers the defenseArm continually while the button is being held down.
- *
- * @author shaina
- *
- */
-public class LowerDefenseArmContinuous extends Command {
-
- private double speed;
-
- public LowerDefenseArmContinuous(double speed) {
- requires(Robot.defenseArm);
- this.speed = -speed;
- }
-
- @Override
- protected void initialize() {
- Robot.defenseArm.setArmSpeed(speed);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- Robot.defenseArm.setArmSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command is intended to be run from OI using button.whileHeld(...).
- * It lowers the defenseWrist continually while the button is being held down.
- *
- * @author shaina
- *
- */
-public class LowerDefenseWristContinuous extends Command {
-
- private double speed;
-
- public LowerDefenseWristContinuous(double speed) {
- requires(Robot.defenseArm);
- this.speed = -speed;
- }
-
- @Override
- protected void initialize() {
- Robot.defenseArm.setHandSpeed(speed);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- Robot.defenseArm.setHandSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command is intended to be run from OI using button.whileHeld(...).
- * It raises the defenseArm continually while the button is being held down.
- *
- * @author shaina
- */
-public class RaiseDefenseArmContinuous extends Command {
-
- private double speed;
-
- public RaiseDefenseArmContinuous(double speed) {
- requires(Robot.defenseArm);
- this.speed = speed;
- }
-
- @Override
- protected void initialize() {
- Robot.defenseArm.setArmSpeed(speed);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- Robot.defenseArm.setArmSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command is intended to be run from OI using button.whileHeld(...).
- * It raises the defenseWrist continually while the button is being held down.
- *
- * @author shaina
- *
- */
-public class RaiseDefenseWristContinuous extends Command {
-
- private double speed;
-
- public RaiseDefenseWristContinuous(double speed) {
- requires(Robot.defenseArm);
- this.speed = speed;
- }
-
- @Override
- protected void initialize() {
- Robot.defenseArm.setHandSpeed(speed);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- Robot.defenseArm.setHandSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/***
- * This command group simultaneously move arm and hand to retracted position.
- *
- * @author shaina
- *
- */
-public class RetractDefenseArm extends CommandGroup {
- static final double ARM_FULLY_RETRACTED_ANGLE = 0; // change value once tested
- static final double HAND_FULLY_RETRACTED_ANGLE = 0; // change value once
- // tested
-
- public RetractDefenseArm(double speed) {
- requires(Robot.defenseArm);
-
- addParallel(new SetArmToAngle(speed, ARM_FULLY_RETRACTED_ANGLE));
- addParallel(new SetHandToAngle(speed, HAND_FULLY_RETRACTED_ANGLE));
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command moves the defense arm to a input angle position.
- * Requires input of a targetPosition and a speed.
- *
- * @author shaina
- *
- */
-public class SetArmToAngle extends Command {
- private static final double THRESHOLD = 0.1;
- private double speed;
- private double targetPosition;
- private double currentPosition;
- private boolean isDecreasing = false;
-
- public SetArmToAngle(double speed, double targetPosition) {
- requires(Robot.defenseArm);
-
- this.speed = speed;
- this.targetPosition = targetPosition;
- }
-
- @Override
- public void initialize() {
- currentPosition = Robot.defenseArm.getArmPotAngle();
-
- if (currentPosition > targetPosition) {
- Robot.defenseArm.setArmSpeed(-speed);
- isDecreasing = true;
- } else {
- Robot.defenseArm.setArmSpeed(speed);
- isDecreasing = false;
- }
- }
-
- @Override
- public void execute() {
-
- }
-
- @Override
- public boolean isFinished() {
- currentPosition = Robot.defenseArm.getArmPotAngle();
-
- if (isDecreasing == true) {
- return (currentPosition <= targetPosition + THRESHOLD);
- } else {
- return (currentPosition >= targetPosition - THRESHOLD);
- }
- }
-
- @Override
- public void end() {
- Robot.defenseArm.setArmSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.defensearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command moves the defense arm to a input angle position.
- * Requires input of a targetPosition and a speed.
- *
- * @author shaina
- *
- */
-public class SetHandToAngle extends Command {
- private static final double THRESHOLD = 0.1;
- private double speed;
- private double targetPosition;
- private double currentPosition;
- private boolean isDecreasing = false;
-
- public SetHandToAngle(double speed, double targetPosition) {
- requires(Robot.defenseArm);
-
- this.speed = speed;
- this.targetPosition = targetPosition;
- }
-
- @Override
- public void initialize() {
- currentPosition = Robot.defenseArm.getHandPotAngle();
-
- if (currentPosition > targetPosition) {
- Robot.defenseArm.setArmSpeed(-speed);
- isDecreasing = true;
- } else {
- Robot.defenseArm.setArmSpeed(speed);
- isDecreasing = false;
- }
- }
-
- @Override
- public void execute() {
-
- }
-
- @Override
- public boolean isFinished() {
- currentPosition = Robot.defenseArm.getHandPotAngle();
-
- if (isDecreasing == true) {
- return (currentPosition <= targetPosition + THRESHOLD);
- } else {
- return (currentPosition >= targetPosition - THRESHOLD);
- }
- }
-
- @Override
- public void end() {
- Robot.defenseArm.setArmSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.driving;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will drive the drivetrain a certain distance in inches
- *
- * @param distance
- * is the distance we want to drive
- * maxTimeOut is a catch just in case the robot malfunctions and never
- * gets to the setpoint
- *
- * @code
- * Repeatedly updates the driveTrain setpoint
- * Finishes when the time goes over maxTimeOut or the driveTrain hits the
- * setpoint
- * end() disables the PID driveTrain
- */
-public class DriveDistance extends Command {
- private double maxTimeOut;
- double distance;
- int count = 0;
-
- public DriveDistance(double distance, double maxTimeOut) {
- requires(Robot.driveTrain);
- this.maxTimeOut = maxTimeOut;
- this.distance = distance;
- }
-
- @Override
- protected void initialize() {
- Robot.driveTrain.resetEncoders();
- }
-
- @Override
- protected void execute() {
- Robot.driveTrain.driveDistance(distance, maxTimeOut);
-
- }
-
- @Override
- protected boolean isFinished() {
- if (timeSinceInitialized() >= maxTimeOut
- || Robot.driveTrain
- .reachedTarget() || Robot.driveTrain.getError() < 1) {
- System.out.println("time: " + timeSinceInitialized());
- Constants.DriveTrain.time = timeSinceInitialized();
- return true;
- }
- return false;
- }
-
- @Override
- protected void end() {
- Robot.driveTrain.disable();
- }
-
- @Override
- protected void interrupted() {
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.driving;\r
-\r
-import org.usfirst.frc.team3501.robot.Robot;\r
-\r
-import edu.wpi.first.wpilibj.Timer;\r
-import edu.wpi.first.wpilibj.command.Command;\r
-\r
-/**\r
- * This command drives the robot for the specified time and specified speed. (If\r
- * a speed is not specified, a default speed is used\r
- *\r
- *\r
- * dependency on subsystems: drivetrain\r
- *\r
- * pre-condition: robot is on\r
- *\r
- * post condition: robot has driven for the specified amount of time\r
- */\r
-public class DriveForTime extends Command {\r
-\r
- private final static double DEFAULT_SPEED = 0.5;\r
- private double speed;\r
- private double seconds;\r
-\r
- private Timer timer;\r
-\r
- public DriveForTime(double seconds, double speed) {\r
- this.seconds = seconds;\r
- this.speed = speed;\r
- }\r
-\r
- public DriveForTime(double seconds) {\r
- this(seconds, DEFAULT_SPEED);\r
- }\r
-\r
- @Override\r
- protected void initialize() {\r
- timer = new Timer();\r
- timer.start();\r
-\r
- Robot.driveTrain.setMotorSpeeds(speed, speed);\r
- }\r
-\r
- @Override\r
- protected void execute() {\r
- }\r
-\r
- @Override\r
- protected boolean isFinished() {\r
- if (timer.get() >= seconds)\r
- return true;\r
- return false;\r
- }\r
-\r
- @Override\r
- protected void end() {\r
- Robot.driveTrain.setMotorSpeeds(0, 0);\r
- }\r
-\r
- @Override\r
- protected void interrupted() {\r
- }\r
-}\r
package org.usfirst.frc.team3501.robot.commands.driving;
+import org.usfirst.frc.team3501.robot.OI;
import org.usfirst.frc.team3501.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
protected void execute() {
double k = (Robot.driveTrain.isFlipped() ? -1 : 1);
// IDK why but the joystick gives positive values for pulling backwards
- double left = -Robot.oi.leftJoystick.getY();
- double right = -Robot.oi.rightJoystick.getY();
+ double left = -OI.leftJoystick.getY();
+ double right = -OI.rightJoystick.getY();
Robot.driveTrain.drive(left * k, right * k);
}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.driving;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class Stop extends Command {
-
- public Stop() {
- }
-
- @Override
- protected void initialize() {
- Robot.driveTrain.stop();
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.driving;
-
-import org.usfirst.frc.team3501.robot.Constants;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- *
- */
-public class Turn180 extends CommandGroup {
-
- public Turn180() {
- addSequential(new TurnForAngle(180, 5));
- Constants.DriveTrain.inverted = !Constants.DriveTrain.inverted;
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.driving;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * @param angle
- * is the setpoint we want to turn for
- * maxTimeOut catch just in case robot malfunctions and never reaches
- * setpoint
- * @initialize resets the Gyro and prints the current mode
- * @code repeatedly sets a new setpoint angle to the motor
- * @end ends when the setpoint is reached.
- */
-public class TurnForAngle extends Command {
- private double maxTimeOut;
- double angle;
-
- public TurnForAngle(double angle, double maxTimeOut) {
- requires(Robot.driveTrain);
- this.maxTimeOut = maxTimeOut;
- this.angle = angle;
- }
-
- @Override
- protected void initialize() {
- Robot.driveTrain.resetGyro();
- System.out.println(Robot.driveTrain.getMode());
- }
-
- @Override
- protected void execute() {
- Robot.driveTrain.turnAngle(angle);
- Robot.driveTrain.printGyroOutput();
- Robot.driveTrain.printOutput();
-
- }
-
- @Override
- protected boolean isFinished() {
- if (timeSinceInitialized() >= maxTimeOut
- || Robot.driveTrain
- .reachedTarget() || Robot.driveTrain.getError() < 8) {
- System.out.println("time: " + timeSinceInitialized());
- Constants.DriveTrain.time = timeSinceInitialized();
- return true;
- }
- return false;
- }
-
- @Override
- protected void end() {
- Robot.driveTrain.disable();
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.driving;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Constants.Direction;
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command turns the robot in a specified direction for a specified
- * duration in seconds.
- *
- * pre-condition: robot is on a flat surface
- *
- * post-condition: robot has turned in the specified direction for the specified
- * time
- *
- * TODO: test for speed/ time constants for specific angles (ex. 30 degrees, 60
- * degrees, 90 degrees)
- *
- * @author Meryem, Avi, and Sarvesh
- *
- */
-
-public class TurnForTime extends Command {
- private Direction direction;
- private double seconds;
- private Timer timer;
- private double speed;
-
- public TurnForTime(double seconds, Direction direction, double speed) {
- this.seconds = seconds;
- this.direction = direction;
- this.speed = speed;
- }
-
- public TurnForTime(double seconds, Direction direction) {
- this(seconds, direction, Constants.Auton.DEFAULT_SPEED);
- }
-
- @Override
- protected void initialize() {
- timer = new Timer();
- timer.start();
-
- if (direction == Direction.RIGHT) {
- Robot.driveTrain.drive(speed, -speed);
- } else if (direction == Direction.LEFT) {
- Robot.driveTrain.drive(-speed, speed);
- }
- }
-
- @Override
- protected void execute() {
-
- }
-
- @Override
- protected boolean isFinished() {
- return (timer.get() >= seconds);
- }
-
- @Override
- protected void end() {
- Robot.driveTrain.drive(0, 0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.intakearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will continually roll the rollers of the intake arm in the
- * outwards direction and stop the rollers once the command is canceled.
- *
- * pre-condition: This command must be called by a button with the method
- * .whileHeld() in OI
- *
- * @author Lauren and Niyati
- *
- */
-
-public class BallRollerExpelContinuous extends Command {
-
- public BallRollerExpelContinuous() {
- requires(Robot.intakeArm);
- }
-
- @Override
- protected void initialize() {
- Robot.intakeArm.outputBall();
- }
-
- @Override
- protected void execute() {
-
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- Robot.intakeArm.stopRollers();
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.intakearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will continually roll the rollers of the intake arm in the
- * inwards direction and stop the rollers once the command is canceled.
- *
- * pre-condition: This command must be called by a button with the method
- * .whileHeld() in OI
- *
- * @author Lauren and Niyati
- *
- */
-
-public class BallRollerIntakeContinuous extends Command {
-
- public BallRollerIntakeContinuous() {
- requires(Robot.intakeArm);
- }
-
- @Override
- protected void initialize() {
- Robot.intakeArm.intakeBall();
- }
-
- @Override
- protected void execute() {
-
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- Robot.intakeArm.stopRollers();
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.intakearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will expel a boulder from the robot, if it is even present to
- * begin with.
- *
- * pre-condition: Intake arm is at correct height and a boulder is present
- * inside the robot.
- *
- * post-condition: A boulder is expelled from inside the robot to the field
- * outside of the robot.
- *
- * @author Lauren and Niyati
- *
- */
-
-public class ExpelBall extends Command {
- private final int TIMEOUT_AMOUNT = 5;
-
- public ExpelBall() {
- requires(Robot.intakeArm);
- }
-
- @Override
- protected void initialize() {
- this.setTimeout(TIMEOUT_AMOUNT);
- if (Robot.shooter.isBallInside())
- Robot.intakeArm.outputBall();
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return (this.isTimedOut() || !Robot.shooter.isBallInside());
- }
-
- @Override
- protected void end() {
- Robot.intakeArm.stopRollers();
- }
-
- @Override
- protected void interrupted() {
-
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.intakearm;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will take a boulder into the robot if there is not a boulder
- * present inside already.
- *
- * pre-condition: Intake arm must be at correct height and a boulder is not
- * present inside the robot.
- *
- * post-condition: A boulder is taken in from the field outside of the robot
- * into the robot.
- *
- * @author Lauren and Niyati
- *
- */
-
-public class IntakeBall extends Command {
- private final int TIMEOUT_AMOUNT = 5;
-
- public IntakeBall() {
- requires(Robot.intakeArm);
- }
-
- @Override
- protected void initialize() {
- this.setTimeout(TIMEOUT_AMOUNT);
- if (!Robot.shooter.isBallInside())
- Robot.intakeArm.intakeBall();
-
- }
-
- @Override
- protected void execute() {
-
- }
-
- @Override
- protected boolean isFinished() {
- return (this.isTimedOut() || Robot.shooter.isBallInside());
- }
-
- @Override
- protected void end() {
- Robot.intakeArm.stopRollers();
-
- }
-
- @Override
- protected void interrupted() {
-
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.scaler;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class ClampBar extends Command {
- private double speed = 0;
-
- public ClampBar(double speed) {
- this.speed = speed;
- }
-
- @Override
- protected void initialize() {
- setTimeout(Constants.Scaler.SECONDS_TO_CLAMP);
- Robot.scaler.runWinch(this.speed);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return isTimedOut();
- }
-
- @Override
- protected void end() {
- Robot.scaler.runWinch(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.scaler;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class ExtendLift extends Command {
-
- public ExtendLift() {
- }
-
- @Override
- protected void initialize() {
- Robot.scaler.liftScissorLift();
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.scaler;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class RetractLift extends Command {
-
- public RetractLift() {
- }
-
- @Override
- protected void initialize() {
- Robot.scaler.lowerScissorLift();
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.scaler;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- * This command will run the winch motor continuously until the button
- * tirggering it is released.
- *
- * pre-condition: This command must be run by a button in OI with method
- * whileHeld(). The robot must be attached to the tower rung.
- *
- * post-condition: winch motor set to a specified speed.
- *
- * @author Lauren
- *
- */
-
-public class RunWinchContinuous extends Command {
- private double winchUpSpeed;
- private int timeoutAmount;
-
- public RunWinchContinuous(double speed, int timeout) {
- requires(Robot.scaler);
- winchUpSpeed = speed;
- timeoutAmount = timeout;
- }
-
- @Override
- protected void initialize() {
- this.setTimeout(timeoutAmount);
- Robot.scaler.runWinch(winchUpSpeed);
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return this.isTimedOut();
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.scaler;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class StopWinch extends Command {
-
- @Override
- protected void initialize() {
- Robot.scaler.stopWinch();
- }
-
- @Override
- protected void execute() {
-
- }
-
- @Override
- protected boolean isFinished() {
- return true;
- }
-
- @Override
- protected void end() {
-
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.scaler;
-
-import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.commands.auton.CompactRobot;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-public class ToggleScaling extends CommandGroup {
- public ToggleScaling() {
- Constants.Scaler.SCALING = !Constants.Scaler.SCALING;
- if (Constants.Scaler.SCALING)
- addSequential(new CompactRobot());
- }
-}
package org.usfirst.frc.team3501.robot.commands.shooter;
import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.WaitCommand;
* catapult.
*/
public Shoot() {
- if (Robot.shooter.usePhotogate()) {
- if (Robot.shooter.isBallInside()) {
- addSequential(new FireCatapult());
- addSequential(new WaitCommand(Constants.Shooter.WAIT_TIME));
- addSequential(new ResetCatapult());
- }
- } else {
- addSequential(new FireCatapult());
- addSequential(new WaitCommand(Constants.Shooter.WAIT_TIME));
- addSequential(new ResetCatapult());
- }
+ addSequential(new FireCatapult());
+ addSequential(new WaitCommand(Constants.Shooter.WAIT_TIME));
+ addSequential(new ResetCatapult());
}
}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.sensors;
-/**
- * Copyright (c) 2015, www.techhounds.com
- * All rights reserved.
- *
- * <p>
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * </p>
- * <ul>
- * <li>Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.</li>
- * <li>Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.</li>
- * <li>Neither the name of the www.techhounds.com nor the
- * names of its contributors may be used to endorse or promote products
- * derived from this software without specific prior written permission.</li>
- * </ul>
- *
- * <p>
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- * </p>
- */
-
-import java.io.File;
-import java.io.IOException;
-import java.io.PrintStream;
-
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * A Java wrapper around the ITC based ITG-3200 triple axis gryo.
- *
- * <p>
- * Typical usage:
- * </p>
- * <ul>
- * <li>Make sure your ITG-3200 is connected to the I2C bus on the roboRIO and
- * mounted flat (so Z axis is used to track direction robot is facing).</li>
- * <li>Construct a single instance of the {@link GyroLib} class to be
- * shared throughout your Robot code.</li>
- * <li>Use the {@link #getRotationZ()} method create "trackers" that allow you
- * to keep track of how much your robot has rotated (direction your robot is
- * facing).</li>
- * </ul>
- * <p>
- * Be aware of the following:
- * </p>
- * <ul>
- * <li>Angles are in signed degrees (both positive and negative values are
- * possible) and not necessarily normalized (large values like -1203 degrees are
- * possible).</li>
- * <li>The {@link #reset} method is called once initially at construction.
- * Reseting the gyro should only be done when your robot is stationary and it
- * may take up to one second. You should not need to do this and should avoid
- * doing this during the autonomous or teleop periods (unless you know your
- * robot won't be moving).</li>
- * <li>There is a background thread that is automatically started that keeps
- * reading and accumulating values from the ITG-3200. You should not need to use
- * the {@link #start()} or {@link #stop()} methods during normal matches.
- * However, if you only use the gyro during the autonomous period, you can use
- * the {@link #stop()} method at the end of the autonomous period to save some
- * CPU.</li>
- * </ul>
- *
- * <h2>Suggested Usage</h2>
- * <p>
- * You should be able to use this class to aid your robot in making relative
- * turns. For example, if you want to create a command which rotates your robot
- * 90 degrees.
- * </p>
- * <ul>
- * <li>In your command's initialize method, use the {@link #getRotationZ()} and
- * the {@link Rotation#zero()} method on the returned {@link Rotation} object to
- * track how much you have turned.</li>
- * <li>Use the {@link Rotation} object as a PID source and/or check the current
- * angle reported by the {@link Rotation} object in your isFinished() method.
- * </li>
- * </ul>
- */
-public final class GyroLib {
-
- /**
- * Object used to monitor robot rotation.
- *
- * <ul>
- * <li>Use this class to track how much your robot has rotated.</li>
- * <li>Use the {@link GyroLib#getRotationZ()} to create an instance
- * to track how much your robot has rotated around the z-axis (direction robot
- * is facing - useful for making turns)..</li>
- * <li>Use the {@link GyroLib#getRotationX()} to create an instance
- * to track how much your robot has rotated around the x-axis (hopefully not
- * much unless you are tipping).</li>
- * <li>Use the {@link GyroLib#getRotationY()} to create an instance
- * to track how much your robot has rotated around the y-axis (hopefully not
- * much unless you are tipping).</li>
- * <li>Use the {@link GyroLib#getRo
- * </ul>
- */
- public final class Rotation implements RotationTracker {
- /** Raw axis accumulator on gyro associated with this rotation tracker. */
- private Accumulator m_axis;
- /**
- * The degrees reported by the accumulator the last time this tracker was
- * zeroed.
- */
- private double m_zeroDeg;
- /**
- * The number of readings reported by the accumulator the last time this
- * tracker was zeroed.
- */
- private int m_zeroCnt;
-
- /**
- * Constructor is protected, instances are created through the
- * {@link GyroLib} methods.
- *
- * @param axis
- * An accumulator from the gyro for the axis to be tracked.
- */
- private Rotation(Accumulator axis) {
- m_axis = axis;
- m_zeroDeg = 0;
- m_zeroCnt = 0;
- }
-
- /**
- * Zero the tracker (sets the current heading/direction as the zero point).
- */
- public void zero() {
- m_zeroDeg = m_axis.getDegrees();
- m_zeroCnt = m_axis.getReadings();
- }
-
- /**
- * Get the number of degrees rotated since last zeroed.
- *
- * @return A signed number of degrees [-INF, +INF].
- */
- public double getAngle() {
- double angle = m_axis.getDegrees() - m_zeroDeg;
- return angle;
- }
-
- /**
- * Get the total number of times the raw values from the gyro have been read
- * since zeroed.
- *
- * @return A diagnostic count that can be used to make sure the angle is
- * still being updated.
- */
- public int getReadings() {
- return m_axis.getReadings() - m_zeroCnt;
- }
-
- /**
- * Returns the last raw (integer) value read from the gyro for the axis.
- *
- * @return An integer value from the ITG-3200 for the associated axis.
- */
- public int getAngleRateRaw() {
- return m_axis.getRaw();
- }
-
- /**
- * Returns the current rotation rate in degrees/second from the last
- * reading.
- *
- * @return How quickly the system is rotating about the axis in
- * degrees/second.
- */
- public double getAngleRate() {
- return getAngleRateRaw() * COUNT_TO_DEGSEC;
- }
-
- /**
- * Returns the angle value from {@link #getAngle()} so object can be used as
- * a source to a PID controller.
- *
- * @return See {@link #getAngle()}.
- *
- * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
- */
- public double pidGet() {
- return getAngle();
- }
-
- public void setPIDSourceType(PIDSourceType pidSource) {
- // TODO Auto-generated method stub
-
- }
-
- public PIDSourceType getPIDSourceType() {
- // TODO Auto-generated method stub
- return null;
- }
- }
-
- //
- // List of I2C registers which the ITG-3200 uses from the datasheet
- //
- private static final byte WHO_AM_I = 0x00;
- private static final byte SMPLRT_DIV = 0x15;
- private static final byte DLPF_FS = 0x16;
- // private static final byte INT_CFG = 0x17;
- // private static final byte INT_STATUS = 0x1A;
- // private static final byte TEMP_OUT_H = 0x1B;
- // private static final byte TEMP_OUT_L = 0x1C;
- private static final byte GYRO_XOUT_H = 0x1D;
- // private static final byte GYRO_XOUT_L = 0x1E;
- // private static final byte GYRO_YOUT_H = 0x1F;
- // private static final byte GYRO_YOUT_L = 0x20;
- // private static final byte GYRO_ZOUT_H = 0x21;
- // private static final byte GYRO_ZOUT_L = 0x22;
-
- //
- // Bit flags used for interrupt operation
- //
-
- /*
- * Set this bit in INT_CFG register for logic level of INT output pin to be
- * low when interrupt is active (leave 0 if you want it high).
- */
- // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7);
-
- /*
- * Set drive type for interrupt to open drain mode, omit if you want push-pull
- * mode (what does this mean?).
- */
- // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6;
-
- /*
- * Interrupt latch mode (remains set until you clear it), omit this flag to
- * get a 0-50us interrupt pulse.
- */
- // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5;
-
- /*
- * Allow any read operation of data to clear the interrupt flag (otherwise it
- * is only cleared after reading status register).
- */
- // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4;
-
- /*
- * Enable interrup when device is ready (PLL ready after changing clock
- * source). Hmmm?
- */
- // private static final byte INT_CFG_RDY_EN = 1 << 3;
-
- /* Enable interrupt when new data is available. */
- // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1;
-
- /* Guess at mode to use if we want to try enabling interrupts. */
- // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN |
- // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN);
-
- //
- // The bit flags that can be set in the DLPF register on the ITG-3200
- // as specified in the ITG-3200 data sheet.
- //
-
- //
- // The low pass filter bandwidth settings
- //
-
- // private static final byte DLPF_LOWPASS_256HZ = 0 << 0;
- private static final byte DLPF_LOWPASS_188HZ = 1 << 0;
- // private static final byte DLPF_LOWPASS_98HZ = 2 << 0;
- // private static final byte DLPF_LOWPASS_42HZ = 3 << 0;
- // private static final byte DLPF_LOWPASS_20HZ = 4 << 0;
- // private static final byte DLPF_LOWPASS_10HZ = 5 << 0;
- // private static final byte DLPF_LOWPASS_5HZ = 6 << 0;
-
- /** Select range of +/-2000 deg/sec. (only range supported). */
- private static final byte DLPF_FS_SEL_2000 = 3 << 3;
-
- /**
- * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high.
- */
- private static final byte itgAddressJumper = 0x69;
-
- /**
- * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low.
- */
- private static final byte itgAddressNoJumper = 0x68;
-
- /** Multiplier to convert raw integer values returned to degrees/sec. */
- private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375);
-
- /** Set this to true for lots of diagnostic output. */
- private static final boolean DEBUG = false;
-
- /**
- * How many sample readings to make to determine the bias value for each axis.
- */
- private static final int MIN_READINGS_TO_SET_BIAS = 50;
-
- /** I2C Address to use to communicate with the ITG-3200. */
- private byte m_addr;
-
- /** I2C object used to communicate with Gyro. */
- private I2C m_i2c;
-
- /**
- * Background thread responsible for accumulating angle data from the sensor.
- */
- private Thread m_thread;
-
- /** Flag used to signal background thread that the gyro should be reset. */
- private boolean m_need_reset;
-
- /** Flag used to signal background thread that it's time to stop. */
- private boolean m_run_thread;
-
- /** Accumulator for rotation around the x-axis. */
- private Accumulator m_x;
-
- /** Accumulator for rotation around the y-axis. */
- private Accumulator m_y;
-
- /** Accumulator for rotation around the z-axis. */
- private Accumulator m_z;
- private int[] m_xBuffer;
- private int[] m_yBuffer;
- private int[] m_zBuffer;
- private int m_cntBuffer;
- private int m_sizeBuffer;
- private double[] m_timeBuffer;
-
- /**
- * Construct a new instance of the ITG-3200 gryo class.
- *
- * <p>
- * IMPORTANT
- *
- * @param port
- * This should be {@link I2C.Port#kOnboard} if the ITG-3200 is
- * connected to the main I2C bus on the roboRIO. This should be
- * {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP
- * port on the roboRIO.
- * @param jumper
- * This should be true if the ITG-3200 has the AD0 jumpered to logic
- * level high and false if not.
- */
- public GyroLib(I2C.Port port, boolean jumper) {
- m_addr = (jumper ? itgAddressJumper : itgAddressNoJumper);
- m_i2c = new I2C(port, m_addr);
- m_thread = new Thread(new Runnable() {
- @Override
- public void run() {
- accumulateData();
- }
- });
- if (DEBUG) {
- check();
- }
- m_x = new Accumulator();
- m_y = new Accumulator();
- m_z = new Accumulator();
- m_need_reset = true;
-
- start();
- }
-
- /**
- * Construct a {@link Rotation} object used to monitor rotation about the
- * Z-axis.
- *
- * @return A rotation object that very useful for checking the direction your
- * robot is facing.
- */
- public Rotation getRotationZ() {
- return new Rotation(m_z);
- }
-
- /**
- * Construct a {@link Rotation} object used to monitor rotation about the
- * X-axis.
- *
- * @return A rotation object that is probably only useful for checking if your
- * robot is starting to tip over.
- */
- public Rotation getRotationX() {
- return new Rotation(m_x);
- }
-
- /**
- * Construct a {@link Rotation} object used to monitor rotation about the
- * Y-axis.
- *
- * @return A rotation object that is probably only useful for checking if your
- * robot is starting to tip over.
- */
- public Rotation getRotationY() {
- return new Rotation(m_y);
- }
-
- /**
- * Returns string representation of the object for debug purposes.
- */
- public String toString() {
- return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]";
- }
-
- /**
- * Dumps information about the state of the Gyro to the smart dashboard.
- *
- * @param tag
- * Short name like "Gyro" to prefix each label with on the dashboard.
- * @param debug
- * Pass true if you want a whole lot of details dumped onto the
- * dashboard, pass false if you just want the direction of each axis
- * and the temperature.
- */
- public void updateDashboard(String tag, boolean debug) {
- SmartDashboard.putNumber(tag + " x-axis degrees", m_x.getDegrees());
- SmartDashboard.putNumber(tag + " y-axis degrees", m_y.getDegrees());
- SmartDashboard.putNumber(tag + " z-axis degrees", m_z.getDegrees());
-
- if (debug) {
- SmartDashboard.putNumber(tag + " x-axis raw", m_x.getRaw());
- SmartDashboard.putNumber(tag + " y-axis raw", m_y.getRaw());
- SmartDashboard.putNumber(tag + " z-axis raw", m_z.getRaw());
-
- SmartDashboard.putNumber(tag + " x-axis count", m_x.getReadings());
- SmartDashboard.putNumber(tag + " y-axis count", m_y.getReadings());
- SmartDashboard.putNumber(tag + " z-axis count", m_z.getReadings());
-
- SmartDashboard.putString(tag + " I2C Address",
- "0x" + Integer.toHexString(m_addr));
- }
- }
-
- /**
- * Internal method that runs in the background thread to accumulate data from
- * the Gyro.
- */
- private void accumulateData() {
- m_run_thread = true;
- int resetCnt = 0;
-
- while (m_run_thread) {
- if (m_need_reset) {
- // Set gyro to the proper mode
- performResetSequence();
-
- // Reset accumulators and set the number of readings to take to
- // compute bias values
- resetCnt = MIN_READINGS_TO_SET_BIAS;
- m_x.reset();
- m_y.reset();
- m_z.reset();
- m_need_reset = false;
- } else {
- // Go read raw values from ITG-3200 and update our accumulators
- readRawAngleBytes();
-
- if (resetCnt > 0) {
- // If we were recently reset, and have made enough initial
- // readings,
- // then go compute and set our new bias (correction) values
- // for each accumulator
- resetCnt--;
- if (resetCnt == 0) {
- m_x.setBiasByAccumulatedValues();
- m_y.setBiasByAccumulatedValues();
- m_z.setBiasByAccumulatedValues();
- }
- }
- }
-
- // Short delay between each reading
- if (m_run_thread) {
- Timer.delay(.01);
- }
- }
- }
-
- /**
- * Singles the gyro's background thread that we want to reset the gyro.
- *
- * <p>
- * You don't typically need to call this during a match. If you do call it,
- * you should only do so when the robot is stationary and will remain
- * stationary for a short time.
- * </p>
- */
- public void reset() {
- m_need_reset = true;
- }
-
- /**
- * Starts up the background thread that accumulates gyro statistics.
- *
- * <p>
- * You never need to call this method unless you have stopped the gyro and now
- * want to start it up again. If you do call this method, you should probably
- * also call the {@link #reset} method.
- * </p>
- */
- public void start() {
- if (!m_thread.isAlive()) {
- m_thread.start();
- }
- }
-
- /**
- * Stops the background thread from accumulating angle information (turns OFF
- * gyro!).
- *
- * <p>
- * This method is not typically called as it stops the gyro from accumulating
- * statistics essentially turning it off. The only time you might want to do
- * this is if you are done using the gyro for the rest of the match and want
- * to save some CPU cyles (for example, if you only needed the gyro during the
- * autonomous period).
- * </p>
- */
- public void stop() {
- m_run_thread = false;
- }
-
- /**
- * Sends commands to configure the ITG-3200 the way we need it to run.
- */
- private void performResetSequence() {
- // Configure the gyroscope
- // Set the gyroscope scale for the outputs to +/-2000 degrees per second
- m_i2c.write(DLPF_FS, (DLPF_FS_SEL_2000 | DLPF_LOWPASS_188HZ));
- // Set the sample rate to 100 hz
- m_i2c.write(SMPLRT_DIV, 9);
- }
-
- /**
- * Enables the buffering of the next "n" data samples (which can then be saved
- * for analysis).
- *
- * @param samples
- * Maximum number of samples to read.
- */
- public void enableBuffer(int samples) {
- double[] timeBuffer = new double[samples];
- int[] xBuffer = new int[samples];
- int[] yBuffer = new int[samples];
- int[] zBuffer = new int[samples];
- synchronized (this) {
- m_timeBuffer = timeBuffer;
- m_xBuffer = xBuffer;
- m_yBuffer = yBuffer;
- m_zBuffer = zBuffer;
- m_cntBuffer = 0;
- m_sizeBuffer = samples;
- }
- }
-
- /**
- * Check to see if the buffer is full.
- *
- * @return true if buffer is at capacity.
- */
- public boolean isBufferFull() {
- boolean isFull;
- synchronized (this) {
- isFull = (m_cntBuffer == m_sizeBuffer);
- }
- return isFull;
- }
-
- /**
- * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for
- * inspection via Excel.
- */
- public void saveBuffer() {
- double[] timeBuffer;
- int[] xBuffer;
- int[] yBuffer;
- int[] zBuffer;
- int size;
-
- // Transfer buffer info to local variables and turn off buffering in a
- // thread safe way.
- synchronized (this) {
- timeBuffer = m_timeBuffer;
- xBuffer = m_xBuffer;
- yBuffer = m_yBuffer;
- zBuffer = m_zBuffer;
- size = m_cntBuffer;
- m_sizeBuffer = 0;
- m_cntBuffer = 0;
- }
-
- if (size > 0) {
- try {
- PrintStream out = new PrintStream(new File("/tmp/gryo-data.csv"));
- out.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\"");
- for (int i = 0; i < size; i++) {
- out.println(timeBuffer[i] + "," + xBuffer[i] + "," + yBuffer[i] + ","
- + zBuffer[i]);
- }
- out.close();
- SmartDashboard.putBoolean("Gyro Save OK", true);
- } catch (IOException ignore) {
- SmartDashboard.putBoolean("Gyro Save OK", false);
- }
- }
- }
-
- /**
- * Internal method run in the background thread that reads values from the
- * ITG-3200 and updates the accumulators.
- */
- private void readRawAngleBytes() {
- double now = Timer.getFPGATimestamp();
-
- byte[] buffer = new byte[6];
- boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer);
-
- if (rc) {
- // Got a good read, get 16 bit integer values for each axis and
- // update accumulated values
- int x = (buffer[0] << 8) | (buffer[1] & 0xff);
- int y = (buffer[2] << 8) | (buffer[3] & 0xff);
- int z = (buffer[4] << 8) | (buffer[5] & 0xff);
-
- m_x.update(x, now);
- m_y.update(y, now);
- m_z.update(z, now);
-
- // If buffered enabled, then save values in a thread safe way
- if (m_sizeBuffer > 0) {
- synchronized (this) {
- int i = m_cntBuffer;
- if (i < m_sizeBuffer) {
- m_timeBuffer[i] = now;
- m_xBuffer[i] = x;
- m_yBuffer[i] = y;
- m_zBuffer[i] = z;
- m_cntBuffer++;
- }
- }
- }
- }
-
- if (DEBUG) {
- String name = toString();
- String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H",
- "ZOUT_L" };
- for (int i = 0; i < labels.length; i++) {
- SmartDashboard.putString(name + " " + labels[i],
- "0x" + Integer.toHexString(0xff & buffer[i]));
- }
- }
- }
-
- /**
- * Helper method to check that we can communicate with the gyro.
- */
- private void check() {
- byte[] buffer = new byte[1];
- boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer);
- if (DEBUG) {
- String name = toString();
- SmartDashboard.putBoolean(name + " Check OK?", rc);
- SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]);
- }
- }
-
- /**
- * Private helper class to accumulate values read from the gryo and convert
- * degs/sec into degrees.
- */
- private class Accumulator {
- /** Accumulated degrees since zero. */
- private double m_accumulatedDegs;
- /**
- * 2 times the computed bias value that is used when getting average of
- * readings.
- */
- private double m_bias2;
- /** The prior raw value read from the gyro. */
- private int m_lastRaw;
- /** The prior time stamp the last raw value was read. */
- private double m_lastTime;
- /** The total count of time the gyro value has been read. */
- private int m_cnt;
- /** The sum of all of the raw values read. */
- private long m_sum;
-
- /** Multipler to covert 2*Count to degrees/sec (optimization). */
- private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0);
-
- /**
- * Returns the accumulated degrees.
- *
- * @return Accumulated signed degrees since last zeroed.
- */
- public synchronized double getDegrees() {
- return m_accumulatedDegs;
- }
-
- /**
- * @return The raw integer reading from the ITG-3200 associated with the
- * axis.
- */
- public int getRaw() {
- return m_lastRaw;
- }
-
- /**
- * Returns the number or readings that went into the accumulated degrees.
- *
- * @return Count of readings since last zeroed.
- */
- public synchronized int getReadings() {
- return m_cnt;
- }
-
- /**
- * Constructs a new instance.
- */
- private Accumulator() {
- m_bias2 = 0;
- zero();
- }
-
- /**
- * Zeros out accumulated information.
- */
- private void zero() {
- m_lastRaw = 0;
- m_lastTime = 0;
- m_sum = 0;
- synchronized (this) {
- m_cnt = 0;
- m_accumulatedDegs = 0;
- }
- }
-
- /**
- * Zeros out accumulated information and clears (zeros) the internal bias
- * value.
- */
- private void reset() {
- zero();
- m_bias2 = 0;
- }
-
- /**
- * Computes new bias value from accumulated values and then zeros.
- */
- private void setBiasByAccumulatedValues() {
- m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt);
- zero();
- }
-
- /**
- * Updates (accumulates) new value read from axis.
- *
- * @param raw
- * Raw signed 16 bit value read from gyro for axis.
- * @param time
- * The time stamp when the value was read.
- */
- private void update(int raw, double time) {
- double degs = 0;
-
- if (m_cnt != 0) {
- // Get average of degrees per second over the time span
- double degPerSec = (m_lastRaw + raw - m_bias2) * COUNT2_TO_DEGSEC;
- // Get time span this rate occurred for
- double secs = (m_lastTime - time);
- // Get number of degrees rotated for time period
- degs = degPerSec * secs;
- }
-
- // Update our thread shared values
- synchronized (this) {
- m_accumulatedDegs += degs;
- m_sum += raw;
- m_cnt++;
- m_lastRaw = raw;
- m_lastTime = time;
- }
- }
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.sensors;
-
-import java.util.TimerTask;
-
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.I2C.Port;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.Timer;
-
-public class Lidar implements PIDSource {
- private I2C i2c;
- private byte[] distance;
- private java.util.Timer updater;
-
- private final int LIDAR_ADDR = 0x62;
- private final int LIDAR_CONFIG_REGISTER = 0x00;
- private final int LIDAR_DISTANCE_REGISTER = 0x8f;
-
- public Lidar(Port port) {
- i2c = new I2C(port, LIDAR_ADDR);
-
- distance = new byte[2];
-
- updater = new java.util.Timer();
- }
-
- // Distance in cm
- public int getDistance() {
- return (int) Integer.toUnsignedLong(distance[0] << 8)
- + Byte.toUnsignedInt(distance[1]);
- }
-
- @Override
- public double pidGet() {
- return getDistance();
- }
-
- // Start 10Hz polling
- public void start() {
- updater.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
- }
-
- // Start polling for period in milliseconds
- public void start(int period) {
- updater.scheduleAtFixedRate(new LIDARUpdater(), 0, period);
- }
-
- public void stop() {
- updater.cancel();
- updater = new java.util.Timer();
- }
-
- // Update distance variable
- public void update() {
- i2c.write(LIDAR_CONFIG_REGISTER, 0x04); // Initiate measurement
- Timer.delay(0.04); // Delay for measurement to be taken
- i2c.read(LIDAR_DISTANCE_REGISTER, 2, distance); // Read in measurement
- Timer.delay(0.005); // Delay to prevent over polling
- }
-
- // Timer task to keep distance updated
- private class LIDARUpdater extends TimerTask {
- @Override
- public void run() {
- while (true) {
- update();
- try {
- Thread.sleep(10);
- } catch (InterruptedException e) {
- e.printStackTrace();
- }
- }
- }
- }
-
- @Override
- public void setPIDSourceType(PIDSourceType pidSource) {
- }
-
- @Override
- public PIDSourceType getPIDSourceType() {
- return null;
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.sensors;
-
-import edu.wpi.first.wpilibj.AnalogInput;
-
-/***
- * The photogate is a pair of IR LED and phototransistor sensor that uses a
- * reflective method to sense the presence of the boulder within the robot's
- * shooting chamber. This class specifically checks for the ball's presence
- * using a threshold of voltages outputted from the phototransistor.
- *
- * @author niyatisriram
- */
-public class Photogate extends AnalogInput {
-
- private double threshold = 1.8;
-
- /***
- * The constructor inputs the channel of the transistor and the threshold
- * value.
- * The threshold is a specific value, representing the outputted voltage of
- * the phototransistor. This value will be somewhere within the range [0,
- * 4095] Find the value by testing and finding an average value for which the
- * ball is present when the output is greater, and absent when the output is
- * less.
- */
- public Photogate(int channel, int threshold) {
- super(channel);
- this.threshold = threshold;
- }
-
- /***
- * @return whether the ball is present or not
- */
- public boolean isBallPresent() {
- if (this.getVoltage() > threshold)
- return true;
- else
- return false;
-
- }
-
- /***
- * @param threshold
- * (range [0, 4095])
- */
- public void setThreshold(int threshold) {
- this.threshold = threshold;
- }
-}
+++ /dev/null
-/**
- * Copyright (c) 2015, www.techhounds.com
- * All rights reserved.
- *
- * <p>
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * </p>
- * <ul>
- * <li>Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.</li>
- * <li>Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.</li>
- * <li>Neither the name of the www.techhounds.com nor the
- * names of its contributors may be used to endorse or promote products
- * derived from this software without specific prior written permission.</li>
- * </ul>
- *
- * <p>
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- * </p>
- */
-
-package org.usfirst.frc.team3501.robot.sensors;
-
-import edu.wpi.first.wpilibj.PIDSource;
-
-/**
- * A rotation tracker allows you to keep track of how far the robot has rotated
- * since the object was constructed or zeroed.
- *
- * <p>
- * This object is most useful when you want to turn your robot a particular
- * number of degrees, or when you want to detect whether your robot is tipping.
- * </p>
- *
- * <p>
- * You will typically use the {@link #getRotationTracker} method associated with
- * the gyro class associated with your hardware. For example
- * {@link GyroItg3200.getRotationTrackerZ()}.
- * </p>
- */
-public interface RotationTracker extends PIDSource {
-
- /**
- * Returns the angle in signed decimal degrees since construction or zeroing.
- *
- * <p>
- * This value is used as the PID sensor value.
- * </p>
- *
- * @return Number of degrees the robot has rotated in the range of [-INF,
- * +INF]. Positive values indicate clockwise rotation (720 means it
- * has spun around twice and is facing the same direction).
- */
- public double getAngle();
-
- /**
- * Zeros the rotation tracker so the current direction we are pointing now
- * becomes the zero point.
- */
- public void zero();
-
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.subsystems;
-
-import org.usfirst.frc.team3501.robot.Constants;
-
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.CANTalon;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-public class DefenseArm extends Subsystem {
- private AnalogPotentiometer defenseArmPotentiometer;
- private AnalogPotentiometer defenseHandPotentiometer;
- private CANTalon defenseArm;
- private CANTalon defenseHand;
- private double hookHeight;
- private double footHeight;
-
- private double[] potHandAngles;
- private double[] potArmAngles;
- private double[] potAngles;
-
- // angles corresponding to pre-determined heights we will need
-
- public DefenseArm() {
- defenseArmPotentiometer = new AnalogPotentiometer(
- Constants.DefenseArm.ARM_CHANNEL, Constants.DefenseArm.FULL_RANGE,
- Constants.DefenseArm.OFFSET);
- defenseHandPotentiometer = new AnalogPotentiometer(
- Constants.DefenseArm.HAND_CHANNEL, Constants.DefenseArm.FULL_RANGE,
- Constants.DefenseArm.OFFSET);
-
- defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT);
- defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT);
- potHandAngles = createHandPotArray();
- potArmAngles = createArmPotArray();
- }
-
- public double getArmPotAngle() {
- return defenseArmPotentiometer.get();
- }
-
- public double getHandPotAngle() {
- return defenseHandPotentiometer.get();
- }
-
- /***
- * This method takes an arm location as input (range of [0,2]) Returns the
- * angle of the arm corresponding to that arm location
- *
- * @param desiredArmLocation
- * takes an arm location ranging from [0,2] 0 is the lowest position
- * of arm 2 is the highest position of arm
- * @return the angle of the arm corresponding to that arm location
- */
- public double getAngleForHandLocation(int desiredArmLocation) {
- return potHandAngles[desiredArmLocation];
- }
-
- public double getAngleForArmLocation(int desiredArmLocation) {
- return potArmAngles[desiredArmLocation];
- }
-
- public double[] createHandPotArray() {
- double[] arr = new double[3];
-
- for (int i = 0; i < 3; i++) {
- arr[i] = 45 * i;
- }
- return arr;
- }
-
- public double[] createArmPotArray() {
- double[] arr = new double[3];
-
- for (int i = 0; i < 3; i++) {
- arr[i] = 45 * i;
- }
- return arr;
- }
-
- /***
- * This method sets the voltage of the arm motor. The range is from [-1,1]. A
- * negative voltage makes the direction of the motor go backwards.
- *
- * @param speed
- * The voltage that you set the motor at. The range of the voltage of
- * the arm motor is from [-1,1]. A negative voltage makes the
- * direction of the motor go backwards.
- */
-
- public void setArmSpeed(double speed) {
- if (speed > 1)
- speed = 1;
- else if (speed < -1)
- speed = -1;
-
- defenseArm.set(speed);
- }
-
- /***
- * This method sets the voltage of the hand motor. The range is from [-1,1]. A
- * negative voltage makes the direction of the motor go backwards.
- *
- * @param speed
- * The voltage that you set the motor at. The range of the voltage of
- * the hand motor is from [-1,1]. A negative voltage makes the
- * direction of the motor go backwards.
- */
-
- public void setHandSpeed(double speed) {
- if (speed > 1)
- speed = 1;
- else if (speed < -1)
- speed = -1;
-
- defenseHand.set(speed);
- }
-
- // TODO: figure out if measurements are all in inches
- public double getArmHorizontalDisplacement() {
- double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH
- * Math.cos(getArmPotAngle());
- double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH
- * Math.cos(getHandPotAngle());
- return (armHorizontalDisplacement + handHorizontalDisplacement);
- }
-
- public double getArmVerticalDisplacement() {
- double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
- double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH
- * Math.sin(getArmPotAngle());
- double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH
- * Math.sin(getHandPotAngle());
- return (armMounted + armVerticalDisplacement + handVerticalDisplacement);
- }
-
- public double getArmHorizontalDist() {
- double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle());
- double hand = Constants.DefenseArm.HAND_LENGTH
- * Math.cos(getHandPotAngle());
- return (arm + hand);
- }
-
- public double getArmHeight() {
- double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
- double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle());
- double hand = Constants.DefenseArm.HAND_LENGTH
- * Math.sin(getHandPotAngle());
- return (armMounted + arm + hand);
- }
-
- public boolean isOutsideRange() {
- if (getArmHorizontalDist() < 15)
- return false;
- return true;
- }
-
- @Override
- protected void initDefaultCommand() {
- }
-}
package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.MathLib;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
-import org.usfirst.frc.team3501.robot.sensors.GyroLib;
-import org.usfirst.frc.team3501.robot.sensors.Lidar;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
public class DriveTrain extends PIDSubsystem {
- // Current Drive Mode Default Drive Mode is Manual
- private int DRIVE_MODE = 1;
private boolean outputFlipped = false;
private static double pidOutput = 0;
private Encoder leftEncoder, rightEncoder;
- public static Lidar lidar;
-
private CANTalon frontLeft, frontRight, rearLeft, rearRight;
private RobotDrive robotDrive;
- private GyroLib gyro;
private DoubleSolenoid leftGearPiston, rightGearPiston;
// Drivetrain specific constants that relate to the inches per pulse value for
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
- lidar = new Lidar(I2C.Port.kMXP);
leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
- gyro = new GyroLib(I2C.Port.kOnboard, false);
-
- DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
- setEncoderPID();
this.disable();
- gyro.start();
leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_MODULE,
Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_MODULE,
Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
- Constants.DriveTrain.inverted = false;
}
@Override
rightEncoder.reset();
}
- public double getLidarDistance() {
- return lidar.pidGet();
- }
-
public double getRightSpeed() {
return rightEncoder.getRate(); // in inches per second
}
// Get error between the setpoint of PID Controller and the current state of
// the robot
public double getError() {
- if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
- return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
- else
- return Math.abs(this.getSetpoint() + getGyroAngle());
- }
-
- public double getGyroAngle() {
- return gyro.getRotationZ().getAngle();
- }
-
- public void resetGyro() {
- gyro.reset();
+ return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
}
public void printEncoder(int i, int n) {
}
}
- public void printGyroOutput() {
- System.out.println("Gyro Angle" + -this.getGyroAngle());
- }
-
/*
* returns the PID output that is returned by the PID Controller
*/
// Updates the PID constants based on which control mode is being used
public void updatePID() {
- if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
- this.getPIDController().setPID(Constants.DriveTrain.kp,
- Constants.DriveTrain.ki, Constants.DriveTrain.kd);
- else
- this.getPIDController().setPID(Constants.DriveTrain.gp,
- Constants.DriveTrain.gd, Constants.DriveTrain.gi);
+ this.getPIDController().setPID(Constants.DriveTrain.kp,
+ Constants.DriveTrain.ki, Constants.DriveTrain.kd);
}
public CANTalon getFrontLeft() {
return rearRight;
}
- public int getMode() {
- return DRIVE_MODE;
- }
-
/*
* Method is a required method that the PID Subsystem uses to return the
* calculated PID value to the driver
protected void usePIDOutput(double output) {
double left = 0;
double right = 0;
- if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
- double drift = this.getLeftDistance() - this.getRightDistance();
- if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
- output = Math.signum(output) * 0.3;
- left = output;
- right = output + drift * Constants.DriveTrain.kp / 10;
- } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
- left = output;
- right = -output;
- }
+ double drift = this.getLeftDistance() - this.getRightDistance();
+ if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
+ output = Math.signum(output) * 0.3;
+ left = output;
+ right = output + drift * Constants.DriveTrain.kp / 10;
drive(left, right);
pidOutput = output;
}
* both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
*/
private double sensorFeedback() {
- if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
- return getAvgEncoderDistance();
- else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
- return -this.getGyroAngle();
- // counterclockwise is positive on joystick but we want it to be negative
- else
- return 0;
+ return getAvgEncoderDistance();
}
/*
*/
public void drive(double left, double right) {
robotDrive.tankDrive(-left, -right);
- // dunno why but inverted drive (- values is forward)
- if (!Constants.DriveTrain.inverted)
- robotDrive.tankDrive(-left, -right);
- else
- robotDrive.tankDrive(right, left);
- }
-
- /*
- * constrains the distance to within -100 and 100 since we aren't going to
- * drive more than 100 inches
- *
- * Configure Encoder PID
- *
- * Sets the setpoint to the PID subsystem
- */
- public void driveDistance(double dist, double maxTimeOut) {
- dist = MathLib.constrain(dist, -100, 100);
- setEncoderPID();
- setSetpoint(dist);
- }
-
- /*
- * Sets the encoder mode Updates the PID constants sets the tolerance and sets
- * output/input ranges Enables the PID controllers
- */
- public void setEncoderPID() {
- DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
- this.updatePID();
- this.setAbsoluteTolerance(Constants.DriveTrain.encoderTolerance);
- this.setOutputRange(-1.0, 1.0);
- this.setInputRange(-200.0, 200.0);
- this.enable();
- }
-
- /*
- * Sets the Gyro Mode Updates the PID constants, sets the tolerance and sets
- * output/input ranges Enables the PID controllers
- */
- private void setGyroPID() {
- DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
- this.updatePID();
- this.getPIDController().setPID(Constants.DriveTrain.gp,
- Constants.DriveTrain.gi, Constants.DriveTrain.gd);
-
- this.setAbsoluteTolerance(Constants.DriveTrain.gyroTolerance);
- this.setOutputRange(-1.0, 1.0);
- this.setInputRange(-360.0, 360.0);
- this.enable();
- }
-
- /*
- * Turning method that should be used repeatedly in a command
- *
- * First constrains the angle to within -360 and 360 since that is as much as
- * we need to turn
- *
- * Configures Gyro PID and sets the setpoint as an angle
- */
- public void turnAngle(double angle) {
- angle = MathLib.constrain(angle, -360, 360);
- setGyroPID();
- setSetpoint(angle);
}
public void setMotorSpeeds(double left, double right) {
+++ /dev/null
-package org.usfirst.frc.team3501.robot.subsystems;
-
-import org.usfirst.frc.team3501.robot.Constants;
-
-import edu.wpi.first.wpilibj.CANTalon;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-public class Scaler extends Subsystem {
- private DoubleSolenoid scaler;
- private CANTalon winch;
-
- public Scaler() {
- scaler = new DoubleSolenoid(Constants.Scaler.FORWARD_CHANNEL,
- Constants.Scaler.REVERSE_CHANNEL);
- winch = new CANTalon(Constants.Scaler.WINCH_MOTOR);
- }
-
- public Value getSolenoidValue() {
- return scaler.get();
- }
-
- public void liftScissorLift() {
- scaler.set(DoubleSolenoid.Value.kReverse);
- }
-
- public void lowerScissorLift() {
- scaler.set(DoubleSolenoid.Value.kForward);
- }
-
- public void runWinch(double speed) {
- if (speed > 1)
- speed = 1;
- if (speed < -1)
- speed = -1;
-
- winch.set(speed);
- }
-
- public void stopWinch() {
- runWinch(Constants.Scaler.WINCH_STOP_SPEED);
- }
-
- @Override
- protected void initDefaultCommand() {
-
- }
-}
package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.sensors.Photogate;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.command.Subsystem;
public class Shooter extends Subsystem {
private DoubleSolenoid catapult1, catapult2;
- private Photogate photogate;
- private boolean usePhotoGate;
public Shooter() {
catapult1 = new DoubleSolenoid(Constants.Shooter.CATAPULT1_MODULE,
catapult2 = new DoubleSolenoid(Constants.Shooter.CATAPULT2_MODULE,
Constants.Shooter.CATAPULT2_FORWARD,
Constants.Shooter.CATAPULT2_REVERSE);
- usePhotoGate = false;
- }
-
- /***
- * This method checks to see if the ball has successfully passed through the
- * intake rollers and is inside.
- *
- * @return whether the presence of the ball is true or false and returns the
- * state of the condition (true or false).
- */
-
- public boolean isBallInside() {
- if (usePhotogate())
- return photogate.isBallPresent();
- else
- return true;
}
// Catapult Commands
catapult2.set(Constants.Shooter.reset);
}
- public boolean usePhotogate() {
- return this.usePhotoGate;
- }
-
- public void togglePhotoGate() {
- this.usePhotoGate = !this.usePhotoGate;
- }
-
@Override
protected void initDefaultCommand() {
}
projects / 3501 / stronghold-2016 / commitdiff