diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt index b12474c..dcd8ade 100644 --- a/AAAScripts/scripts.txt +++ b/AAAScripts/scripts.txt @@ -36,7 +36,7 @@ RxLx: #------------------------------------------------- #Exchange -RxxE +RxxE: moveto (0, 188.4) (-210.6, 188.4) (-210.6, 44.4) (-123.6, 44.4) (-123.6, -39.6) 180 #Curve around switch for baseline #then go to exchange exchange @@ -144,7 +144,7 @@ LxRx: #------------------------------------------------- #Exchange -LxxE +LxxE: moveto (0, 188.4) (210.6, 188.4) (210.6, 3.7) (124.8, 3.7) (124.8, -3.3) 180 #Curve around switch for baseline then go to exchange exchange @@ -244,13 +244,13 @@ moveto (48,80.375) #cross baseline CRxx: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length +moveto (48,104.375) #7 feet forward until against switch enclosure, compensating for robot length switch #deploy switch CLxx: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #4 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length switch #deploy switch @@ -275,7 +275,7 @@ scale #deploy scale CRLx: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure +moveto (48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn -90 #turn -90 degrees @@ -294,7 +294,7 @@ scale #deploy scale2 CRRx: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure +moveto (48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn -90 #turn -90 degrees @@ -313,7 +313,7 @@ scale #deploy scale CLRx: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #4 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure +moveto (-48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn -90 #turn -90 degrees @@ -331,7 +331,7 @@ scale #deploy scale CLLx: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #4 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure +moveto (-48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn 90 #turn 90 degrees @@ -352,7 +352,7 @@ scale #deploy scale2 CRxE: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length +moveto (48,104.375) #7 feet forward until against switch enclosure, compensating for robot length switch #deploy switch move -24 # 2 feet back turn -90 @@ -368,7 +368,7 @@ exchange CLxE: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #3 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length switch #deploy switch move -24 turn 90 @@ -383,7 +383,7 @@ exchange CxxE: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #3 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length move -96 #move 8 feet back turn 180 #turn 180 degrees to face the exchange exchange \ No newline at end of file diff --git a/Robot2018/lib/User_Libraries.properties b/Robot2018/lib/User_Libraries.properties index 0e4f44a..1ee5cb2 100644 --- a/Robot2018/lib/User_Libraries.properties +++ b/Robot2018/lib/User_Libraries.properties @@ -1,2 +1,2 @@ -#Sun Feb 18 15:40:25 PST 2018 +#Sun Feb 18 17:40:43 PST 2018 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 7e4e616..ced2550 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -7,6 +7,11 @@ package org.usfirst.frc.team199.Robot2018; +import org.usfirst.frc.team199.Robot2018.commands.CloseIntake; +import org.usfirst.frc.team199.Robot2018.commands.IntakeCube; +import org.usfirst.frc.team199.Robot2018.commands.LowerIntake; +import org.usfirst.frc.team199.Robot2018.commands.OpenIntake; +import org.usfirst.frc.team199.Robot2018.commands.OutakeCube; import org.usfirst.frc.team199.Robot2018.commands.PIDMove; import org.usfirst.frc.team199.Robot2018.commands.PIDTurn; import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders; @@ -15,6 +20,8 @@ import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType; import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear; import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear; +import org.usfirst.frc.team199.Robot2018.commands.ToggleLeftIntake; +import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake; import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants; import edu.wpi.first.wpilibj.Joystick; @@ -50,6 +57,8 @@ public class OI { private JoystickButton lowerIntake; private JoystickButton intake; private JoystickButton outake; + private JoystickButton toggleLeftIntake; + private JoystickButton toggleRightIntake; public int getButton(String key, int def) { if (!SmartDashboard.containsKey("Button/" + key)) { @@ -90,22 +99,24 @@ public OI() { MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); - // manipulator = new Joystick(2); - // closeIntake = new JoystickButton(manipulator, getButton("Close Intake - // Button", 1)); - // closeIntake.whenPressed(new CloseIntake()); - // openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", - // 2)); - // openIntake.whenPressed(new OpenIntake()); + manipulator = new Joystick(2); + closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1)); + closeIntake.whenPressed(new CloseIntake()); + openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2)); + openIntake.whenPressed(new OpenIntake()); // raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake // Button", 3)); // raiseIntake.whenPressed(new RaiseIntake()); // lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake // Button", 4)); - // lowerIntake.whenPressed(new LowerIntake()); - // intake = new JoystickButton(manipulator, getButton("Intake Button", 5)); - // intake.whenPressed(new IntakeCube()); - // outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); - // outake.whenPressed(new OutakeCube()); + lowerIntake.whenPressed(new LowerIntake()); + intake = new JoystickButton(manipulator, getButton("Intake Button", 5)); + intake.whenPressed(new IntakeCube()); + outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); + outake.whenPressed(new OutakeCube()); + toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3)); + toggleLeftIntake.whenPressed(new ToggleLeftIntake()); + toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4)); + toggleRightIntake.whenPressed(new ToggleRightIntake()); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 5c21509..b24495e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -169,8 +169,8 @@ public void autonomousInit() { strategies.put(key, chooser.getSelected()); } - Autonomous auto = new Autonomous(startPos, strategies, autoDelay, fmsInput, false); - auto.start(); + Scheduler.getInstance().add(new Autonomous(startPos, strategies, autoDelay, fmsInput, false)); + // auto.start(); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index a627bdc..339e086 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -130,12 +130,10 @@ public RobotMap() { leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 9)); rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 8)); - // leftIntakeHorizontalSolenoid = new - // DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2), - // getPort("IntakeLeftHorizontalSolenoidPort2", 3)); - // rightIntakeHorizontalSolenoid = new - // DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4), - // getPort("IntakeRightHorizontalSolenoidPort2", 5)); + leftIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2), + getPort("IntakeLeftHorizontalSolenoidPort2", 3)); + rightIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4), + getPort("IntakeRightHorizontalSolenoidPort2", 5)); // leftIntakeVerticalSolenoid = new // DoubleSolenoid(getPort("IntakeLeftVerticalSolenoidPort1", 6), // getPort("IntakeLeftVerticalSolenoidPort2", 7)); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java index 7f523a3..3d16272 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -24,15 +24,16 @@ public static Map> parseScriptFile(String scriptFile ArrayList currScript = new ArrayList(); String currScriptName = ""; - int count = 1; + int count = 0; for (String line : lines) { + count++; // remove comments int commentIndex = line.indexOf("#"); if (commentIndex != -1) line = line.substring(0, commentIndex); // trim and remove extra whitespace just to make it neater - line = line.trim().replaceAll("\\s+", " "); + line = line.trim().replaceAll("\\s+", " "); // make coordinates with spaces also work int parenIndex = line.indexOf("("); while (parenIndex != -1) { @@ -40,7 +41,7 @@ public static Map> parseScriptFile(String scriptFile int endParenIndex = line.indexOf(")", parenIndex); String coord = line.substring(parenIndex + 1, endParenIndex); line = line.substring(0, parenIndex + 1) + coord.replaceAll(" ", "") + line.substring(endParenIndex); - + // finds next parentheses parenIndex = line.indexOf("(", parenIndex + 1); } @@ -76,7 +77,7 @@ public static Map> parseScriptFile(String scriptFile currScript.add(command); } } - count++; + } // puts the last script in @@ -187,7 +188,7 @@ else if (instruction.equals("jump")) { * the message to log */ - private static void logWarning (int lineNumber, String message) { + private static void logWarning(int lineNumber, String message) { System.err.println("[ERROR] Line " + lineNumber + ": " + message); } @@ -246,13 +247,13 @@ public static double[] parsePoint(String cmdArgs) { if (AutoUtils.isPoint(cmdArgs)) { parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1); pointparts = parentheseless.split(","); - try { - point[0] = Double.parseDouble(pointparts[0]); - point[1] = Double.parseDouble(pointparts[1]); - } catch (Exception e) { - point[0] = 1; - point[1] = 1; - } + try { + point[0] = Double.parseDouble(pointparts[0]); + point[1] = Double.parseDouble(pointparts[1]); + } catch (Exception e) { + point[0] = 1; + point[1] = 1; + } } return point; } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java deleted file mode 100644 index 6ba1dd2..0000000 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.usfirst.frc.team199.Robot2018.commands; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class AutoMove extends Command { - - public AutoMove(double distance) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java index ec04c61..76e0333 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -24,25 +24,11 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface for (String arg : args) { if (AutoUtils.isDouble(arg)) { rotation = Double.valueOf(arg); - double relrotation = rotation - AutoUtils.position.getRot(); - addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc)); - AutoUtils.position.setRot(rotation); + addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true)); } else if (AutoUtils.isPoint(arg)) { point = AutoUtils.parsePoint(arg); - double deltaX = point[0] - AutoUtils.position.getX(); - double deltaY = point[1] - AutoUtils.position.getY(); - double atan = Math.toDegrees(Math.atan(deltaX / deltaY)); - double relrotation = atan - AutoUtils.position.getRot(); - addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc)); - double dX2 = deltaX * deltaX; - double dY2 = deltaY * deltaY; - double distance = Math.sqrt(dX2 + dY2); - addSequential(new PIDMove(distance, dt, sd, pidMoveSrc)); - double x = AutoUtils.position.getX(); - double y = AutoUtils.position.getY(); - AutoUtils.position.setX(point[0]); - AutoUtils.position.setY(point[1]); - AutoUtils.position.setRot(Math.toDegrees(Math.atan((point[0] - x) / (point[1] - y)))); + addSequential(new PIDTurn(point, dt, sd, pidMoveSrc)); + addSequential(new PIDMove(point, dt, sd, pidMoveSrc)); } else { throw new IllegalArgumentException(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java deleted file mode 100644 index 1ce32a5..0000000 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.usfirst.frc.team199.Robot2018.commands; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class AutoTurn extends Command { - - public AutoTurn(double angle) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java index 5557563..fa127ba 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java @@ -20,7 +20,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.intakeEject.runIntake(1); + Robot.intakeEject.runIntake(-1); } // Make this return true when this Command no longer needs to run execute() diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java index e68dea4..ee95d98 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java @@ -20,24 +20,28 @@ public OutakeCube() { // Called just before this Command runs the first time protected void initialize() { tim = new Timer(); + tim.reset(); + tim.start(); } // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.intakeEject.runIntake(-1); + Robot.intakeEject.runIntake(1); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return tim.get() > Robot.getConst("Outake Time", 1); + return tim.get() > Robot.getConst("Outake Time", 0.5); } // Called once after isFinished returns true protected void end() { + Robot.intakeEject.runIntake(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() { + end(); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index 11ab9da..c1a0b76 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; import edu.wpi.first.wpilibj.PIDController; @@ -32,6 +33,8 @@ public class PIDMove extends Command implements PIDOutput { * @param dt * the Drivetrain (for actual code) or a DrivetrainInterface (for * testing) + * @param sd + * the SmartDashboard * @param avg * the PIDSourceAverage of the DT's two Encoders */ @@ -49,6 +52,40 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, SmartDashboard.putData("Move PID", moveController); } + /** + * Constructs this command with a new PIDController. Sets all of the + * controller's PID constants based on SD prefs. Sets the controller's PIDSource + * to the encoder average object and sets its PIDOutput to this command which + * implements PIDOutput's pidWrite() method. + * + * @param point + * the target point in inches, absolute distance from the starting + * point + * @param dt + * the Drivetrain (for actual code) or a DrivetrainInterface (for + * testing) + * @param sd + * the SmartDashboard + * @param avg + * the PIDSorceAverage of the DT's two Encoders + */ + public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) { + double dx = point[0] - AutoUtils.position.getX(); + double dy = point[1] - AutoUtils.position.getY(); + + double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance + + this.target = dist; + this.dt = dt; + this.avg = avg; + if (Robot.dt != null) { + requires(Robot.dt); + } + double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05)); + moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), + kf, avg, this); + } + /** * Called just before this Command runs the first time. Resets the distance * encoders, sets the moveController's settings, and then enables it. @@ -63,7 +100,7 @@ public void initialize() { moveController.setOutputRange(-1.0, 1.0); moveController.setContinuous(false); moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1)); - moveController.setSetpoint(Robot.getConst("Move Targ", 24)); + moveController.setSetpoint(target); moveController.enable(); // dt.enableVelocityPIDs(); @@ -104,6 +141,13 @@ protected void end() { moveController.disable(); System.out.println("End"); // moveController.free(); + + double angle = Math.toRadians(AutoUtils.position.getRot()); + double dist = avg.pidGet(); + double x = Math.cos(angle) * dist; + double y = Math.sin(angle) * dist; + AutoUtils.position.changeX(x); + AutoUtils.position.changeY(y); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 183aca3..04bc3b0 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; import edu.wpi.first.wpilibj.PIDController; @@ -41,13 +42,82 @@ public class PIDTurn extends Command implements PIDOutput { * @param sd * the Smart Dashboard reference, or a SmartDashboardInterface for * testing + * @param absolute + * whether the target passed is absolute or relative */ - public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { + public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs, boolean absolute) { // Use requires() here to declare subsystem dependencies - target = targ; this.dt = dt; this.ahrs = ahrs; + if (absolute) { + target = targ - AutoUtils.position.getRot(); + } else { + target = targ; + } + + if (Robot.dt != null) { + requires(Robot.dt); + } + // calculates the maximum turning speed in degrees/sec based on the max linear + // speed in inches/s and the distance (inches) between sides of the DT + double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25)); + double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05)); + turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0), + kf, ahrs, this); + // tim = new Timer(); + } + + /** + * Constructs this command with a new PIDController. Sets all of the + * controller's PID constants based on SD prefs. Sets the controller's PIDSource + * to the AHRS (gyro) object and sets its PIDOutput to this command which + * implements PIDOutput's pidWrite() method. + * + * @param targ + * the target bearing (in degrees) to turn to (so negative if turning + * left, positive if turning right) + * @param dt + * the Drivetrain (for actual code) or a DrivetrainInterface (for + * testing) + * @param ahrs + * the AHRS (gyro) + * @param sd + * the Smart Dashboard reference, or a SmartDashboardInterface for + * testing + */ + public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { + this(targ, dt, sd, ahrs, false); + } + + /** + * Constructs this command with a new PIDController. Sets all of the + * controller's PID constants based on SD prefs. Sets the controller's PIDSource + * to the AHRS (gyro) object and sets its PIDOutput to this command which + * implements PIDOutput's pidWrite() method. + * + * @param point + * the target point (in inches) to turn to, relative to the starting + * position + * @param dt + * the Drivetrain (for actual code) or a DrivetrainInterface (for + * testing) + * @param ahrs + * the AHRS (gyro) + * @param sd + * the Smart Dashboard reference, or a SmartDashboardInterface for + * testing + */ + public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { + this.dt = dt; + this.ahrs = ahrs; + + double dx = point[0] - AutoUtils.position.getX(); + double dy = point[1] - AutoUtils.position.getY(); + + double absTurn = Math.toDegrees(Math.atan2(dy, dx)); + target = absTurn - AutoUtils.position.getRot(); + if (Robot.dt != null) { requires(Robot.dt); } @@ -79,7 +149,7 @@ protected void initialize() { turnController.setOutputRange(Robot.getConst("Output", 0.5) * -1, Robot.getConst("Output", 0.5)); turnController.setContinuous(true); turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1)); - double newSetPoint = Robot.getConst("Turn Targ", 90) + dt.getAHRSAngle(); + double newSetPoint = target + dt.getAHRSAngle(); while (Math.abs(newSetPoint) > 180) { newSetPoint = newSetPoint - Math.signum(newSetPoint) * 360; } @@ -136,6 +206,8 @@ protected void end() { SmartDashboard.putNumber("Turn PID Result", turnController.get()); SmartDashboard.putNumber("Turn PID Error", turnController.getError()); // turnController.free(); + + AutoUtils.position.changeRot(dt.getAHRSAngle()); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java index 2c0bc59..bef69bf 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java @@ -25,16 +25,17 @@ public RunScript(String scriptName) { addSequential(new AutoMoveTo(cmdArgs.split(" "))); break; case "turn": - double rotation = Double.parseDouble(cmdArgs); - addSequential(new PIDTurn(rotation, Robot.dt, Robot.sd, Robot.dt.getGyro())); - AutoUtils.position.setRot(rotation); + if (AutoUtils.isPoint(cmdArgs)) { + double[] point = AutoUtils.parsePoint(cmdArgs); + addSequential(new PIDTurn(point, Robot.dt, Robot.sd, Robot.dt.getGyro())); + } else { + double rotation = Double.parseDouble(cmdArgs); + addSequential(new PIDTurn(rotation, Robot.dt, Robot.sd, Robot.dt.getGyro())); + } break; case "move": double distance = Double.parseDouble(cmdArgs); - addSequential(new PIDMove(distance, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg())); - AutoUtils.position.setX(distance * Math.sin(Math.toRadians(AutoUtils.position.getRot()))); - AutoUtils.position.setY(distance * Math.cos(Math.toRadians(AutoUtils.position.getRot()))); break; case "switch": addSequential(new EjectToSwitch()); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java new file mode 100644 index 0000000..6a4daec --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +/** + * + */ +public class ToggleLeftIntake extends InstantCommand { + + public ToggleLeftIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.intakeEject); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.toggleLeftIntake(); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java new file mode 100644 index 0000000..e03d274 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +/** + * + */ +public class ToggleRightIntake extends InstantCommand { + + public ToggleRightIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.intakeEject); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.toggleRightIntake(); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java index 77cdfbf..926e3df 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * @@ -48,9 +49,9 @@ public double getRightIntakeSpeed() { * */ public boolean hasCube() { - return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 38) - && pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current", - 38); + return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 39) + || pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current", + 39); } /** @@ -124,29 +125,67 @@ public void lowerIntake() { } /** - * Opens the intake + * Toggles the left intake between open and closed */ - public void openIntake() { + public void toggleLeftIntake() { + DoubleSolenoid.Value set; + if (Robot.getBool("Bool/Left Horizontal Solenoid Open", true)) { + set = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + } else { + set = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + } + leftHorizontalSolenoid.set(set); + SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", + Robot.getBool("Left Horizontal Solenoid Open", true)); + } + + /** + * Toggles the right intake between open and closed + */ + public void toggleRightIntake() { + DoubleSolenoid.Value set; + if (Robot.getBool("Bool/Right Horizontal Solenoid Open", true)) { + set = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + } else { + set = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + } + rightHorizontalSolenoid.set(set); + SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", + Robot.getBool("right Horizontal Solenoid Open", true)); + } + + /** + * Closes the intake + */ + public void closeIntake() { DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward; DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward; + SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", true); + SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", true); leftHorizontalSolenoid.set(leftSet); rightHorizontalSolenoid.set(rightSet); } /** - * Closes the intake + * Opens the intake */ - public void closeIntake() { + public void openIntake() { DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward : DoubleSolenoid.Value.kReverse; DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward : DoubleSolenoid.Value.kReverse; + SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", false); + SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", false); leftHorizontalSolenoid.set(leftSet); rightHorizontalSolenoid.set(rightSet); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java index f37bdde..01f5eb1 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java @@ -64,6 +64,16 @@ public interface IntakeEjectInterface { */ public void lowerIntake(); + /** + * Toggles the left intake between open and closed + */ + public void toggleLeftIntake(); + + /** + * Toggles the right intake between open and closed + */ + public void toggleRightIntake(); + /** * Closes the intake */