From d64e0d6930af22cc1ca7cc3c75a311166d321d4c Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Mon, 20 Feb 2017 14:23:53 -0800 Subject: [PATCH] fix stuff --- src/org/usfirst/frc/team3501/robot/Robot.java | 5 +- ...AutonGearThenBaselinePegCloseToBoiler.java | 101 ++++++++---------- .../robot/commands/driving/DriveDistance.java | 3 +- .../robot/commands/driving/TurnForAngle.java | 1 + .../team3501/robot/subsystems/DriveTrain.java | 8 +- 5 files changed, 57 insertions(+), 61 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index c446a86..515483c 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,5 +1,6 @@ package org.usfirst.frc.team3501.robot; +import org.usfirst.frc.team3501.robot.commandgroups.AutonGearThenBaselinePegCloseToBoiler; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.Intake; import org.usfirst.frc.team3501.robot.subsystems.Shooter; @@ -47,6 +48,8 @@ public class Robot extends IterativeRobot { @Override public void autonomousInit() { driveTrain.setHighGear(); + Scheduler.getInstance() + .add(new AutonGearThenBaselinePegCloseToBoiler("RETRIEVAL", 0)); } @Override @@ -56,7 +59,7 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { - + Scheduler.getInstance().removeAll(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java index 6af22a4..2fcb8f3 100644 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java @@ -17,17 +17,15 @@ public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup { - private static final double ROBOT_LENGTH = 40.0; - private static final double ROBOT_WIDTH = 36.0; + private static final double ROBOT_LENGTH = 36.0; + private static final double ROBOT_WIDTH = 40.0; - private static final double DISTANCE_TO_PEG_EXTENDED_BOILER = 73.1657; - private static final double DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE = 114.3 - + 17.625 - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3)); - private static final double FIRST_LIMIT_LENGTH = 75.649; - private static final double SECOND_LIMIT_LENGTH = 162.8289106736; + private static final double STRAIGHT_DIST_FROM_BOILER = 76.8; + private static final double DISTANCE_TO_RETRIEVAL_PEG = 114.3 + 17.625 + - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3)); private static final double LENGTH_OF_PEG = 10.5; private static final double PEG_EXTENDED_BOILER = 117.518; - private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE + private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_RETRIEVAL_PEG * 2) / Math.sqrt(3); private static final double DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE = 255.6765151902; private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not @@ -38,64 +36,57 @@ public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup // arena in inches (can change) /*** - * - * @param team + * + * @param side * which side to start on: "BLUE" for blue side and "RED" for red * side - * @param distanceFromBoilerCorner + * @param distanceFromcorner * distance from the corner of the boiler */ - public AutonGearThenBaselinePegCloseToBoiler(String team, - int distanceFromBoilerCorner) { - - this.distanceFromCorner = distanceFromBoilerCorner; + public AutonGearThenBaselinePegCloseToBoiler(String side, + int distanceFromcorner) { requires(Robot.getDriveTrain()); - if (distanceFromBoilerCorner >= 0 - && distanceFromBoilerCorner < FIRST_LIMIT_LENGTH) { - // boiler peg path - // drive straight into extended line of peg - addSequential( - new DriveDistance( - ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3)) - + DISTANCE_TO_PEG_EXTENDED_BOILER - (ROBOT_LENGTH / 2.0), - MOTOR_VALUE_FORWARD)); + if (side.equals("BOILER")) { + addSequential(new DriveDistance( + 131.6 - (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) + - ROBOT_LENGTH / 2, + 5)); + // addSequential(new DriveDistance( + // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3)) + // + STRAIGHT_DIST_FROM_BOILER - (ROBOT_LENGTH / 2.0), + // MOTOR_VALUE_FORWARD)); // position robot for optimal gear placement - addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN)); + addSequential(new TurnForAngle(60, Direction.RIGHT, 5)); // put gear on peg - addSequential(new DriveDistance(PEG_EXTENDED_BOILER - - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2) - / Math.sqrt(3)) - - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), - MOTOR_VALUE_FORWARD)); + addSequential(new DriveDistance( + 2 * (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) + - ROBOT_LENGTH / 2 + 7, + 5)); + // addSequential(new DriveDistance(PEG_EXTENDED_BOILER + // - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2) / Math.sqrt(3)) + // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD)); // does not include drift distance - } else if (distanceFromBoilerCorner > SECOND_LIMIT_LENGTH - && distanceFromBoilerCorner <= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE) // mirror - // of - // right - // peg - // path - { - this.distanceFromCorner = DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE - - distanceFromBoilerCorner; - // Turning DISTANCE_FROM_BOILER_WALL into starting point from left side by - // reversing it - addSequential( - new DriveDistance( - ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3)) - + DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE - - (ROBOT_LENGTH / 2.0), - MOTOR_VALUE_FORWARD)); - addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN)); - addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE - - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2) - - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), - MOTOR_VALUE_FORWARD)); + } else if (side.equals("RETRIEVAL")) { + addSequential(new DriveDistance( + 131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) + - ROBOT_LENGTH / 2, + 5)); + addSequential(new TurnForAngle(60, Direction.LEFT, 5)); + addSequential(new DriveDistance( + 2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) + - ROBOT_LENGTH / 2, + 5)); + // addSequential(new DriveDistance( + // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3)) + // + DISTANCE_TO_RETRIEVAL_PEG - (ROBOT_LENGTH / 2.0), + // MOTOR_VALUE_FORWARD)); + // addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN)); + // addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE + // - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2) + // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD)); // does not include drift distance - } else { - System.out.println( - "DO NOT START HERE!!! MOVE CLOSER TO A WALL OR LOSE ALL AUTON POINTS!!"); } } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java index 582cd80..d297e66 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java @@ -47,12 +47,13 @@ public class DriveDistance extends Command { this.driveTrain.resetEncoders(); this.driveController.setSetPoint(this.target); this.zeroAngle = driveTrain.getAngle(); + System.out.println("drive " + target); } @Override protected void execute() { double xVal = gyroP * (driveTrain.getAngle() - zeroAngle); - double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance()); + double yVal = driveController.calcPID(driveTrain.getLeftEncoderDistance()); double leftDrive = yVal - xVal; double rightDrive = yVal + xVal; diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java index 717f151..44434c6 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java @@ -70,6 +70,7 @@ public class TurnForAngle extends Command { } this.driveTrain.setMotorValues(leftDrive, rightDrive); + System.out.println(this.driveTrain.getAngle() - this.zeroAngle); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 32ec7b2..a0b75c6 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -14,8 +14,8 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.Subsystem; public class DriveTrain extends Subsystem { - public static double driveP = 0.006, driveI = 0.0011, driveD = -0.002; - public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005; + public static double driveP = 0.008, driveI = 0.0011, driveD = -0.002; + public static double turnP = 0.006, turnI = 0.0013, turnD = -0.005; public static double driveStraightGyroP = 0.01; public static final double WHEEL_DIAMETER = 4; // inches @@ -77,8 +77,8 @@ public class DriveTrain extends Subsystem { // DRIVE METHODS public void setMotorValues(double left, double right) { - left = MathLib.restrictToRange(left, 0.0, 1.0); - right = MathLib.restrictToRange(right, 0.0, 1.0); + left = MathLib.restrictToRange(left, -1.0, 1.0); + right = MathLib.restrictToRange(right, -1.0, 1.0); frontLeft.set(left); rearLeft.set(left); -- 2.30.2