From 93453536a87a1f0decf9a88ab00f77438097b5a7 Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Mon, 15 Feb 2016 16:01:09 -0800 Subject: [PATCH] fix command names and get rid of unused stuff --- .../team3501/robot/commands/AlignToScore.java | 47 +++++++-------- .../robot/commands/auton/AlignToScore.java | 59 ++++++++++--------- .../robot/commands/shooter/Punch.java | 2 +- .../team3501/robot/subsystems/Shooter.java | 17 +----- 4 files changed, 56 insertions(+), 69 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java index a2f83d42..356c71a8 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java +++ b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands; -import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; +import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -19,6 +20,7 @@ public class AlignToScore extends CommandGroup { private final double DIST_CENTER_OF_MASS_TO_FRONT_OF_ROBOT = 0; private final double DEFAULT_SPEED = 0.5; + private final double maxTimeout = 5; // constants for position 1: low bar private final double POS1_DIST1 = 0; @@ -56,42 +58,37 @@ public class AlignToScore extends CommandGroup { // position 1 is always the low bar case 1: - addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS1_TURN1)); - addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS1_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED)); case 2: - addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS2_TURN1)); - addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS2_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED)); case 3: - addSequential(new DriveForDistance(POS3_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS3_TURN1)); - addSequential(new DriveForDistance(POS3_DIST2, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS3_TURN2)); - addSequential(new DriveForDistance(POS3_DIST3, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS3_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS3_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED)); case 4: - addSequential(new DriveForDistance(POS4_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS4_TURN1)); - addSequential(new DriveForDistance(POS4_DIST2, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS4_TURN2)); - addSequential(new DriveForDistance(POS4_DIST3, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS4_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS4_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED)); case 5: - addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS5_TURN1)); - addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS5_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED)); } } - - public static void calculatePath() { - double leftDistance = Robot.shooter.getLeftDistanceToTower(); - double rightDistance = Robot.shooter.getRightDistanceToTower(); - } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java index 918a9fca..0f7a79b2 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java @@ -1,6 +1,5 @@ package org.usfirst.frc.team3501.robot.commands.auton; -import org.usfirst.frc.team3501.robot.Robot; import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; @@ -116,31 +115,35 @@ public class AlignToScore extends CommandGroup { } } - 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; - } + // 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; + * } + */ } diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java index deb2c04b..36ef785f 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/Punch.java @@ -8,7 +8,7 @@ public class Punch extends Command { @Override protected void initialize() { - Robot.shooter.punch(); + Robot.shooter.extendPunch(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index 22239e49..0264c757 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -2,9 +2,7 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.Lidar; -import org.usfirst.frc.team3501.robot.MathLib; -import edu.wpi.first.wpilibj.AnalogPotentiometer; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -29,12 +27,10 @@ public class Shooter extends Subsystem { private Lidar lidar; public Shooter() { - leftLidar = new AnalogPotentiometer(0); - rightLidar = new AnalogPotentiometer(0); shooter = new CANTalon(Constants.Shooter.PORT); angleAdjuster = new CANTalon(Constants.Shooter.ANGLE_ADJUSTER_PORT); - punch = new DoubleSolenoid(Constants.Shooter.PUNCH_FORWARD_PORT, - Constants.Shooter.PUNCH_REVERSE_PORT); + punch = new DoubleSolenoid(Constants.Shooter.PUNCH_FORWARD, + Constants.Shooter.PUNCH_REVERSE); encoder = new Encoder(Constants.Shooter.ENCODER_PORT_A, Constants.Shooter.ENCODER_PORT_B, false, EncodingType.k4X); @@ -88,13 +84,4 @@ public class Shooter extends Subsystem { @Override protected void initDefaultCommand() { } - - public double getLeftDistanceToTower() { - // TODO: find the method that actually gets distance - return leftLidar.get(); - } - - public double getRightDistanceToTower() { - return rightLidar.get(); - } } -- 2.30.2