diff --git a/src/main/java/org/usfirst/frc/team1700/robot/Robot.java b/src/main/java/org/usfirst/frc/team1700/robot/Robot.java index 297a459..ea3941c 100644 --- a/src/main/java/org/usfirst/frc/team1700/robot/Robot.java +++ b/src/main/java/org/usfirst/frc/team1700/robot/Robot.java @@ -27,13 +27,13 @@ public class Robot extends IterativeRobot { public static AutonomousBase auto; // Control variables - double leftIntakeSpeed = 0; - double rightIntakeSpeed = 0; - Boolean desiredGrabIntakeState = false; - Boolean desiredFoldIntakeState = true; - double leftSpeed = 0; - double rightSpeed = 0; - double elevatorSpeed = 0; + public static double leftIntakeSpeed = 0; + public static double rightIntakeSpeed = 0; + public static Boolean desiredGrabIntakeState = RobotMap.GRAB_INTAKE_CLOSE; + public static Boolean desiredFoldIntakeState = true; + public static double leftSpeed = 0; + public static double rightSpeed = 0; + public static double elevatorSpeed = 0; // State variables Boolean hasGameData = false; @@ -65,7 +65,8 @@ public void disabledPeriodic() { public void autonomousInit() { // Choose auton mode based on starting location Boolean preferSwitch = true; - auto = new AutonomousBase(AutonomousBase.StartLocation.LEFT, preferSwitch); + + auto = new AutonomousBase(AutonomousBase.StartLocation.LEFT, preferSwitch, driveSubsystem, intakeSubsystem, elevatorSubsystem); } /** @@ -74,6 +75,13 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { auto.periodic(); + + // EXECUTE (Autonomous responsible for setting all of these values) + driveSubsystem.driveTank(leftSpeed, rightSpeed); + elevatorSubsystem.elevatorMove(elevatorSpeed); + intakeSubsystem.fold(desiredFoldIntakeState); + intakeSubsystem.grab(desiredGrabIntakeState); + intakeSubsystem.runIntake(leftIntakeSpeed, rightIntakeSpeed); } @Override diff --git a/src/main/java/org/usfirst/frc/team1700/robot/autonmodes/AutonomousBase.java b/src/main/java/org/usfirst/frc/team1700/robot/autonmodes/AutonomousBase.java index 9908dbc..8a1731a 100644 --- a/src/main/java/org/usfirst/frc/team1700/robot/autonmodes/AutonomousBase.java +++ b/src/main/java/org/usfirst/frc/team1700/robot/autonmodes/AutonomousBase.java @@ -6,29 +6,38 @@ import java.time.Instant; import edu.wpi.first.wpilibj.DriverStation; +import org.usfirst.frc.team1700.robot.Robot; +import org.usfirst.frc.team1700.robot.subsystems.DriveSubsystem; +import org.usfirst.frc.team1700.robot.subsystems.ElevatorSubsystem; +import org.usfirst.frc.team1700.robot.subsystems.IntakeSubsystem; + public class AutonomousBase{ // Auto game data / decision making variables - public enum StartLocation { + public static enum StartLocation { LEFT, CENTER, RIGHT; } - protected enum GameDataState { + protected static enum GameDataState { WAITING, RECEIVED, TIMEOUT; } - protected enum AutoDecisionState { + protected static enum AutoDecisionState { SWITCH, SCALE, CROSS_SCALE, CROSS_LINE; } + private DriveSubsystem driveSubsystem; + private ElevatorSubsystem elevatorSubsystem; + private IntakeSubsystem intakeSubsystem; + Instant start; GameDataState gameDataState = GameDataState.WAITING; AutoDecisionState autoDecisionState; @@ -37,9 +46,70 @@ protected enum AutoDecisionState { String gameData; Boolean preferSwitch; - public AutonomousBase(StartLocation inputLocation, Boolean inputPreferSwitch) { + // Autonomous constants + public static final long waitTime = 250; // milliseconds + // All distances in inches + public static final double distToSwitch = 140; + public static final double distToAutoLine = 110; + public static final double finalDistToSwitch = 30; + public static final double finalDistToScale = 23; // with bad encoder: 55 + + public static final double crossScaleDist1 = 210; + public static final double crossScaleDist2 = 190; + public static final double crossScaleDist3 = 57; + + public static final double sameScaleDist = 226; // with bad encoder: 280 + + public static final double crossSwitchDist1 = 18; + public static final double crossSwitchDist2 = 127; + public static final double crossSwitchDist3 = 18; + + public static final double centerLeftDist1 = 50; + public static final double centerLeftDist2 = 80; + public static final double centerLeftDist3 = 20; + + public static final double centerRightDist1 = 50; + public static final double centerRightDist2 = 70; + public static final double centerRightDist3 = 25; + + public static final double centerDistBack = -18; + + public static final double sameSwitchDist = 140; + + public static final double left = -90; + public static final double right = 90; + + // Auto progression constants + private static final double angleTolerance = 4; + private static final double distanceTolerance = 3; + private static final double velocityTolerance = 0.1; + private static final double angleVelocityTolerance = 1; + + private static final double elevatorTolerance = 5; //change later + + // Autonomous variables + // Drivetrain info + double desiredAngle; + double desiredDistance; + double currentDistance; + double currentVelocity; + double currentAngle; + double currentAngleVelocity; + double distanceError; + double angleError; + + // Elevator Info + double desiredElevatorPosition; + double currentElevatorPosition; + double currentElevatorVelocity; + double elevatorError; + + public AutonomousBase(StartLocation inputLocation, Boolean inputPreferSwitch, DriveSubsystem drive, IntakeSubsystem intake, ElevatorSubsystem elevator) { startLocation = inputLocation; preferSwitch = inputPreferSwitch; + driveSubsystem = drive; + intakeSubsystem = intake; + elevatorSubsystem = elevator; } public void periodic(){ @@ -53,9 +123,20 @@ public void periodic(){ makeDecision(); } } - // gameDataState is update so this can check again (similar to else if) + // gameDataState is updated so this can check again (similar to else if) if(gameDataState != GameDataState.WAITING){ - + if(autoDecisionState == AutoDecisionState.CROSS_LINE){ + + } + else if(autoDecisionState == AutoDecisionState.SWITCH){ + + } + else if(autoDecisionState == AutoDecisionState.SCALE){ + + } + else{ + + } } } @@ -84,10 +165,12 @@ protected void makeDecision(){ if(gameData.charAt(0) == 'L'){ // Left side, left switch auto autoDecisionState = AutoDecisionState.SWITCH; + switchAutoInit(); } else if(gameData.charAt(1) == 'L'){ // Left side, left scale auto autoDecisionState = AutoDecisionState.SCALE; + scaleAutoInit(); } else{ autoDecisionState = crossSideDefault; @@ -97,10 +180,12 @@ else if(gameData.charAt(1) == 'L'){ if(gameData.charAt(1) == 'L'){ // Left side, left scale auto autoDecisionState = AutoDecisionState.SCALE; + scaleAutoInit(); } else if(gameData.charAt(0) == 'L'){ // Left side, left switch auto autoDecisionState = AutoDecisionState.SWITCH; + switchAutoInit(); } else{ autoDecisionState = crossSideDefault; @@ -137,12 +222,278 @@ else if(gameData.charAt(0) == 'R'){ } else{ if(preferSwitch){ - autoDecisionState = AutoDecisionState.SWITCH; + // Center switch autons not implemented + } + else{ + // Center scale autons not implemented + } + } + } + + protected enum CrossTheLineStates{ + DRIVING, + END; + } + // crossTheLineAuto parameters + CrossTheLineStates crossTheLineState; + protected void crossTheLineAutoInit(){ + crossTheLineState = CrossTheLineStates.DRIVING; + desiredAngle = 0; + desiredDistance = distToAutoLine; + } + protected void crossTheLineAutoPeriodic(){ + // Read sensors + currentDistance = driveSubsystem.getRightEncoderValue(); + currentVelocity = driveSubsystem.getVelocity(); + currentAngle = driveSubsystem.getNavXAngle(DriveSubsystem.AngleType.YAW); + currentAngleVelocity = 0; + + distanceError = currentDistance - desiredDistance; + angleError = (desiredAngle - currentAngle + 720 + 180) % 360 - 180; + + // Update state + if(crossTheLineState == CrossTheLineStates.DRIVING){ + if(Math.abs(distanceError) maxAngleSpeed) { + angleSpeed = maxAngleSpeed; + } else if (calcAngleSpeed < -maxAngleSpeed) { + angleSpeed = -maxAngleSpeed; + } else { + angleSpeed = Math.copySign(Math.max(minTurnVoltage, Math.abs(calcAngleSpeed)), angleError); + } + + // DISTANCE SPEED + double calcDistSpeed = driveP*distanceError + driveD*distanceVelocity; + double distanceSpeed; + if (calcDistSpeed > maxDistanceSpeed) { + distanceSpeed = maxDistanceSpeed; + } else if (calcDistSpeed < -maxDistanceSpeed) { + distanceSpeed = -maxDistanceSpeed; + } else { + distanceSpeed = Math.copySign(Math.max(minDriveVoltage, Math.abs(calcDistSpeed)), distanceError); + } + + Robot.leftSpeed = -(distanceSpeed + angleSpeed); + Robot.rightSpeed = -(distanceSpeed - angleSpeed); + } + + // Elevator PID constants + double maxSpeed = 0.88; + double elevatorP = 0.01; // not tuned + double elevatorD = 0.000; // not tuned + + private void elevatorPID(double elevatorError, double elevatorVelocity){ + double calcVel = -elevatorP*elevatorError + elevatorD*elevatorVelocity; + + if (Math.abs(elevatorError) <= elevatorTolerance) { + Robot.elevatorSpeed = elevatorSubsystem.stallSpeed; + } + else if (calcVel > maxSpeed) { + Robot.elevatorSpeed = maxSpeed; + } + else if (calcVel < -maxSpeed) { + Robot.elevatorSpeed = -maxSpeed; + } + else { + Robot.elevatorSpeed = calcVel; + } } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc/team1700/robot/subsystems/DriveSubsystem.java b/src/main/java/org/usfirst/frc/team1700/robot/subsystems/DriveSubsystem.java index 5967174..966e409 100644 --- a/src/main/java/org/usfirst/frc/team1700/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/org/usfirst/frc/team1700/robot/subsystems/DriveSubsystem.java @@ -9,47 +9,10 @@ import edu.wpi.first.wpilibj.Encoder; -/** - * - */ -// ITERATIVE CONVERSION NOTE: This class doesn't need to extend anything public class DriveSubsystem{ // AUTO CONSTANTS public static final double inchesToTicks = 11.94; // 40 ticks/in. * 54/20*50/12 gearbox reduction / (12pi in/shaft rotation) - public static final double distToSwitch = 140*inchesToTicks; - public static final double distToAutoLine = 110*inchesToTicks; - public static double finalDistToSwitch = 30*inchesToTicks; - public static double finalDistToScale = 23*inchesToTicks; // with bad encoder: 55 - - public static final double crossScaleDist1 = 210*DriveSubsystem.inchesToTicks; - public static final double crossScaleDist2 = 190*DriveSubsystem.inchesToTicks; - public static final double crossScaleDist3 = 57*DriveSubsystem.inchesToTicks; - - public static final double sameScaleDist = 226*DriveSubsystem.inchesToTicks; // with bad encoder: 280 - - public static final long waitTime = 250; - - public static final double crossSwitchDist1 = 18*DriveSubsystem.inchesToTicks; - public static final double crossSwitchDist2 = 127*DriveSubsystem.inchesToTicks; - public static final double crossSwitchDist3 = 18*DriveSubsystem.inchesToTicks; - - public static final double centerLeftDist1 = 50*DriveSubsystem.inchesToTicks; - public static final double centerLeftDist2 = 80*DriveSubsystem.inchesToTicks; - public static final double centerLeftDist3 = 20*DriveSubsystem.inchesToTicks; - - public static final double centerRightDist1 = 50*DriveSubsystem.inchesToTicks; - public static final double centerRightDist2 = 70*DriveSubsystem.inchesToTicks; - public static final double centerRightDist3 = 25*DriveSubsystem.inchesToTicks; - - public static final double centerDistBack = -18*DriveSubsystem.inchesToTicks; - - public static final double sameSwitchDist = 140*DriveSubsystem.inchesToTicks; - - public static final double left = -90; - public static final double right = 90; - - public int test = 1; // MOTORS AND SENSORS VictorSPX LF = RobotMap.leftFrontDrive; @@ -94,14 +57,14 @@ public void resetNavX() { navx.resetDisplacement(); } - public int getLeftEncoderValue() { - return LE.get(); + public double getLeftEncoderValue() { + return LE.get()*inchesToTicks; } - public int getRightEncoderValue() { - return RE.get(); + public double getRightEncoderValue() { + return RE.get()*inchesToTicks; } - + public void resetEncoders() { LE.reset(); RE.reset(); @@ -111,8 +74,8 @@ public boolean nearZero(double number, double tolerance) { return (Math.abs(number)