diff --git a/src/main/java/frc/team555/robot/Robot.java b/src/main/java/frc/team555/robot/Robot.java index b1531a9..477300f 100644 --- a/src/main/java/frc/team555/robot/Robot.java +++ b/src/main/java/frc/team555/robot/Robot.java @@ -1,13 +1,10 @@ package frc.team555.robot; -import frc.team555.robot.core.PIDLoopTest; + import frc.team555.robot.core.PowerUpRobot; -import frc.team555.robot.core.PowerUpRobot2; -import frc.team555.robot.test.EncoderTestRobot; import frc.team555.robot.test.TestRobot; import frc.team555.robot.test.TestRobot2; -import frc.team555.robot.test.TestRobot3; -public class Robot extends EncoderTestRobot { +public class Robot extends PowerUpRobot { } diff --git a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java new file mode 100644 index 0000000..d92bcd9 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java @@ -0,0 +1,77 @@ +package frc.team555.robot.auto; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.team555.robot.components.Lift; +import frc.team555.robot.components.MainLift; +import frc.team555.robot.core.PowerUpRobot; +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.DriveTime; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.states.StateMachine; + +import static frc.team555.robot.components.MainLift.TOP; + +public class AutoClimbSequence extends StateMachine { + + private Lift lift; + private double height; + private double power; + private boolean up; + + public AutoClimbSequence(MainLift lift){ + super( + new DriveEncoderGyro(10, + 0.4, + Angle.ZERO, + true, + PowerUpRobot.correction + + ), + + new MoveLift(lift, + TOP, + 1, + true + ), + + new DriveTime(3,-0.5), + new MoveLift(lift, + TOP*0.3, + 1, + false + ) + + ); + } + + State MoveLift = new State() { + @Override + public void start() { + power=Math.abs(power); + if(!up) + { + power=-power; + } + } + + @Override + public void stop() + { + lift.setAuto(0); + } + + @Override + public void stateUpdate() { + lift.setAuto(power); + } + + @Override + public boolean isDone() { + return lift.getEncoder().getInches().get()>height==up; + } + }; + + +} diff --git a/src/main/java/frc/team555/robot/auto/AutoStart.java b/src/main/java/frc/team555/robot/auto/AutoStart.java new file mode 100644 index 0000000..7343de4 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoStart.java @@ -0,0 +1,11 @@ +package frc.team555.robot.auto; + +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; +import org.montclairrobotics.sprocket.states.StateMachine; +@Deprecated +public class AutoStart extends StateMachine { + public AutoStart(GyroCorrection correction){ + super(new ResetGyro(correction)); + } +} diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitch.java b/src/main/java/frc/team555/robot/auto/AutoSwitch.java new file mode 100644 index 0000000..883d766 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitch.java @@ -0,0 +1,22 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.states.State; +/* +public abstract class AutoSwitch extends AutoMode { + /*protected static double POWER = 0.75; + + // public AutoSwitch(String name, State... states) { + super(name, states); + } + + public static AutoSwitch fromMiddle() { + return new AutoSwitchMid(Side.fromDriverStation()[0]); + } + + public static AutoSwitch fromSide(Side pos) { + return new AutoSwitchLR(pos, Side.fromDriverStation()[0]); + } +} +*/ \ No newline at end of file diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java new file mode 100644 index 0000000..666a847 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -0,0 +1,33 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.core.PowerUpRobot; +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.utils.Input; +/* +class AutoSwitchLR extends AutoSwitch { + AutoSwitchLR(Side pos, Side target) { + super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")", + /* Reset gyro + new ResetGyro(PowerUpRobot.correction), + /* Drive forward 12 inches + new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Turn π/2 radians (toward the nearest wall) + new DriveEncoderGyro(0,(pos == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction), + /* Drive forward 36 inches + new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Turn π/2 radians (in the opposite direction) + new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction), + /* Drive forward 36 inches + new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Turn π/2 radians (in the opposite direction) + new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction), + /* Drive forward 12 inches OR 72 inches, depending on the target + new DriveEncoderGyro(pos == target ? 12 : 72, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Move the lift, turn, and drop the cube into the switch + new DropCubeSwitch(new Input(){public Side get(){return pos;}}, PowerUpRobot.correction) + ); + } +}*/ diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java new file mode 100644 index 0000000..3334ff6 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -0,0 +1,26 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.core.PowerUpRobot; +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.utils.Input; +/* +class AutoSwitchMid extends AutoSwitch { + + AutoSwitchMid(Side target) { + super("Cube to " + target.toString() + " Switch (M)", + /* Reset gyro + new ResetGyro(PowerUpRobot.correction), + /* Drive forward 12 inches + new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Turn π/2 radians (in the correct directon) + new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER, true, PowerUpRobot.correction), + /* Drive forward 36 inches + new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Move the lift, turn, and drop the cube into the switch + new DropCubeSwitch(new Input(){public Side get(){return target;}}, PowerUpRobot.correction) + ); + } +}*/ diff --git a/src/main/java/frc/team555/robot/auto/AutoWillTest.java b/src/main/java/frc/team555/robot/auto/AutoWillTest.java new file mode 100644 index 0000000..6b675ba --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoWillTest.java @@ -0,0 +1,34 @@ +//package frc.team555.robot.auto; +// +//import frc.team555.robot.core.PowerUpRobot; +//import frc.team555.robot.utils.Side; +//import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +//import org.montclairrobotics.sprocket.auto.states.ResetGyro; +//import org.montclairrobotics.sprocket.geometry.Angle; +//import org.montclairrobotics.sprocket.utils.Input; +// +//public class AutoWillTest extends AutoSwitch { +// +// public AutoWillTest(Side pos, Side target) { +// super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")", +// /* Reset gyro */ +// new ResetGyro(PowerUpRobot.correction), +// /* Drive forward 12 inches */ +// new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction), +// /* Turn π/2 radians (toward the nearest wall) */ +// new DriveEncoderGyro(0,(pos == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction), +// /* Drive forward 36 inches */ +// new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), +// /* Turn π/2 radians (in the opposite direction) */ +// new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction), +// /* Drive forward 36 inches */ +// new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), +// /* Turn π/2 radians (in the opposite direction) */ +// new DriveEncoderGyro(0,(pos == Side.LEFT ? +1 : -1) * POWER, Angle.QUARTER,true, PowerUpRobot.correction), +// /* Drive forward 12 inches OR 72 inches, depending on the target */ +// new DriveEncoderGyro(pos == target ? 12 : 72, POWER, Angle.ZERO, true, PowerUpRobot.correction), +// /* Move the lift, turn, and drop the cube into the switch */ +// new DropCubeSwitch(new Input(){public Side get(){return pos;}}, PowerUpRobot.correction)); +// /* Back away from switch +// } +//} diff --git a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java index 702ad22..14d6c02 100644 --- a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java +++ b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java @@ -2,6 +2,7 @@ import frc.team555.robot.components.CubeIntake; import frc.team555.robot.components.IntakeLift; +import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; @@ -9,18 +10,20 @@ import org.montclairrobotics.sprocket.states.MultiState; import org.montclairrobotics.sprocket.states.State; import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.utils.Input; public class DropCubeSwitch extends StateMachine { private CubeIntake intake; private IntakeLift lift; - public DropCubeSwitch(CubeIntake intake, IntakeLift lift, Side side, GyroCorrection correction){ - super(new MultiState(0, - new SideTurn(correction,false, side), - new IntakeLiftMove(10, .75, true) - ), + public DropCubeSwitch(Input side, GyroCorrection correction){ + super( + new MultiState(0, + new SideTurn(correction,false, side), + new IntakeLiftMove(10, .75, true) + ), new DriveEncoderGyro(3, .5, Angle.ZERO, true, correction), - new CubeOuttake(intake, 1)); + new CubeOuttake(PowerUpRobot.intake, 1)); } } diff --git a/src/main/java/frc/team555/robot/auto/DropCubeTop.java b/src/main/java/frc/team555/robot/auto/DropCubeTop.java new file mode 100644 index 0000000..f68d313 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/DropCubeTop.java @@ -0,0 +1,22 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.AutoState; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.DriveTime; +import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.utils.Input; + +public class DropCubeTop extends StateMachine { + public DropCubeTop(CubeIntake intake, GyroCorrection correction, Input side){ + super( + new DriveEncoderGyro(140, .5, Angle.ZERO, false, correction), + new SideTurn(correction, false, side), + // new MainLiftMove(12,.5,true), + new DriveTime(2, .75) + ); + } +} diff --git a/src/main/java/frc/team555/robot/auto/GRIP/YellowPipeline.java b/src/main/java/frc/team555/robot/auto/GRIP/YellowPipeline.java new file mode 100644 index 0000000..26bd027 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/GRIP/YellowPipeline.java @@ -0,0 +1,248 @@ +package frc.team555.robot.auto.GRIP; + +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import java.util.stream.Collectors; +import java.util.HashMap; + +import edu.wpi.first.wpilibj.vision.VisionPipeline; + +import org.opencv.core.*; +import org.opencv.core.Core.*; +import org.opencv.features2d.FeatureDetector; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.*; +import org.opencv.objdetect.*; + +/** + * YellowGripPipeline class. + * + *

An OpenCV pipeline generated by GRIP. + * + * @author GRIP + */ +public class YellowPipeline implements VisionPipeline { + + //Outputs + private Mat blurOutput = new Mat(); + private Mat hslThresholdOutput = new Mat(); + private ArrayList findContoursOutput = new ArrayList(); + private ArrayList filterContoursOutput = new ArrayList(); + + static { + System.loadLibrary(Core.NATIVE_LIBRARY_NAME); + } + + /** + * This is the primary method that runs the entire pipeline and updates the outputs. + */ + @Override public void process(Mat source0) { + // Step Blur0: + Mat blurInput = source0; + BlurType blurType = BlurType.get("Box Blur"); + double blurRadius = 23.423421705091325; + blur(blurInput, blurType, blurRadius, blurOutput); + + // Step HSL_Threshold0: + Mat hslThresholdInput = blurOutput; + double[] hslThresholdHue = {15.107911610774856, 55.085326184998614}; + double[] hslThresholdSaturation = {90.19784209110755, 252.09897057595106}; + double[] hslThresholdLuminance = {56.74141247216769, 209.58467253996548}; + hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput); + + // Step Find_Contours0: + Mat findContoursInput = hslThresholdOutput; + boolean findContoursExternalOnly = false; + findContours(findContoursInput, findContoursExternalOnly, findContoursOutput); + + } + + /** + * This method is a generated getter for the output of a Blur. + * @return Mat output from Blur. + */ + public Mat blurOutput() { + return blurOutput; + } + + /** + * This method is a generated getter for the output of a HSL_Threshold. + * @return Mat output from HSL_Threshold. + */ + public Mat hslThresholdOutput() { + return hslThresholdOutput; + } + + /** + * This method is a generated getter for the output of a Find_Contours. + * @return ArrayList output from Find_Contours. + */ + public ArrayList findContoursOutput() { + return findContoursOutput; + } + + /** + * This method is a generated getter for the output of a Filter_Contours. + * @return ArrayList output from Filter_Contours. + */ + public ArrayList filterContoursOutput() { + return filterContoursOutput; + } + + + /** + * An indication of which type of filter to use for a blur. + * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL + */ + enum BlurType{ + BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"), + BILATERAL("Bilateral Filter"); + + private final String label; + + BlurType(String label) { + this.label = label; + } + + public static BlurType get(String type) { + if (BILATERAL.label.equals(type)) { + return BILATERAL; + } + else if (GAUSSIAN.label.equals(type)) { + return GAUSSIAN; + } + else if (MEDIAN.label.equals(type)) { + return MEDIAN; + } + else { + return BOX; + } + } + + @Override + public String toString() { + return this.label; + } + } + + /** + * Softens an image using one of several filters. + * @param input The image on which to perform the blur. + * @param type The blurType to perform. + * @param doubleRadius The radius for the blur. + * @param output The image in which to store the output. + */ + private void blur(Mat input, BlurType type, double doubleRadius, + Mat output) { + int radius = (int)(doubleRadius + 0.5); + int kernelSize; + switch(type){ + case BOX: + kernelSize = 2 * radius + 1; + Imgproc.blur(input, output, new Size(kernelSize, kernelSize)); + break; + case GAUSSIAN: + kernelSize = 6 * radius + 1; + Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius); + break; + case MEDIAN: + kernelSize = 2 * radius + 1; + Imgproc.medianBlur(input, output, kernelSize); + break; + case BILATERAL: + Imgproc.bilateralFilter(input, output, -1, radius, radius); + break; + } + } + + /** + * Segment an image based on hue, saturation, and luminance ranges. + * + * @param input The image on which to perform the HSL threshold. + * @param hue The min and max hue + * @param sat The min and max saturation + * @param lum The min and max luminance + */ + private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum, + Mat out) { + Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS); + Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]), + new Scalar(hue[1], lum[1], sat[1]), out); + } + + /** + * Sets the values of pixels in a binary image to their distance to the nearest black pixel. + * @param input The image on which to perform the Distance Transform. + */ + private void findContours(Mat input, boolean externalOnly, + List contours) { + Mat hierarchy = new Mat(); + contours.clear(); + int mode; + if (externalOnly) { + mode = Imgproc.RETR_EXTERNAL; + } + else { + mode = Imgproc.RETR_LIST; + } + int method = Imgproc.CHAIN_APPROX_SIMPLE; + Imgproc.findContours(input, contours, hierarchy, mode, method); + } + + + /** + * Filters out contours that do not meet certain criteria. + * @param inputContours is the input list of contours + * @param output is the the output list of contours + * @param minArea is the minimum area of a contour that will be kept + * @param minPerimeter is the minimum perimeter of a contour that will be kept + * @param minWidth minimum width of a contour + * @param maxWidth maximum width + * @param minHeight minimum height + * @param maxHeight maximimum height + * @param minVertexCount minimum vertex Count of the contours + * @param maxVertexCount maximum vertex Count + * @param minRatio minimum ratio of width to height + * @param maxRatio maximum ratio of width to height + */ + private void filterContours(List inputContours, double minArea, + double minPerimeter, double minWidth, double maxWidth, double minHeight, double + maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double + minRatio, double maxRatio, List output) { + final MatOfInt hull = new MatOfInt(); + output.clear(); + //operation + for (int i = 0; i < inputContours.size(); i++) { + final MatOfPoint contour = inputContours.get(i); + final Rect bb = Imgproc.boundingRect(contour); + if (bb.width < minWidth || bb.width > maxWidth) continue; + if (bb.height < minHeight || bb.height > maxHeight) continue; + final double area = Imgproc.contourArea(contour); + if (area < minArea) continue; + if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue; + Imgproc.convexHull(contour, hull); + MatOfPoint mopHull = new MatOfPoint(); + mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2); + for (int j = 0; j < hull.size().height; j++) { + int index = (int)hull.get(j, 0)[0]; + double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]}; + mopHull.put(j, 0, point); + } + final double solid = 100 * area / Imgproc.contourArea(mopHull); + if (solid < solidity[0] || solid > solidity[1]) continue; + if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue; + final double ratio = bb.width / (double)bb.height; + if (ratio < minRatio || ratio > maxRatio) continue; + output.add(contour); + } + } + + + + +} + diff --git a/src/main/java/frc/team555/robot/auto/MainLiftMove.java b/src/main/java/frc/team555/robot/auto/MainLiftMove.java index 9a0bf75..49c4c43 100644 --- a/src/main/java/frc/team555/robot/auto/MainLiftMove.java +++ b/src/main/java/frc/team555/robot/auto/MainLiftMove.java @@ -5,6 +5,7 @@ import org.montclairrobotics.sprocket.auto.AutoState; import org.montclairrobotics.sprocket.states.State; +@Deprecated public class MainLiftMove implements State { private double height; diff --git a/src/main/java/frc/team555/robot/auto/MoveLift.java b/src/main/java/frc/team555/robot/auto/MoveLift.java new file mode 100644 index 0000000..4d5746c --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/MoveLift.java @@ -0,0 +1,48 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.components.Lift; +import frc.team555.robot.core.Hardware; +import org.montclairrobotics.sprocket.states.State; + +public class MoveLift implements State +{ + private Lift lift; + private double height; + private double power; + private boolean up; + + public MoveLift(Lift lift, double height, double power, boolean up) + { + this.lift=lift; + this.height=height; + this.power=power; + this.up=up; + } + + + @Override + public void start() { + power=Math.abs(power); + if(!up) + { + power=-power; + } + } + + @Override + public void stop() + { + lift.setAuto(0); + } + + @Override + public void stateUpdate() { + lift.setAuto(power); + } + + @Override + public boolean isDone() { + return lift.getEncoder().getInches().get()>height==up; + } + +} diff --git a/src/main/java/frc/team555/robot/auto/OldDropCube.java b/src/main/java/frc/team555/robot/auto/OldDropCube.java deleted file mode 100644 index 3c50f1d..0000000 --- a/src/main/java/frc/team555/robot/auto/OldDropCube.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.team555.robot.auto; - -import frc.team555.robot.components.CubeIntake; -import frc.team555.robot.components.MainLift; -import frc.team555.robot.utils.Side; -import org.montclairrobotics.sprocket.auto.states.DriveTime; -import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; -import org.montclairrobotics.sprocket.states.StateMachine; - - -@Deprecated -public class OldDropCube extends StateMachine{ - public OldDropCube(CubeIntake intake, MainLift lift, GyroCorrection correction, Side side){ - super( - new SideTurn(correction, false, side), - new MainLiftMove(12,.5,true), - new DriveTime(3,.3), - new CubeOuttake(intake, 5) - ); - } -} diff --git a/src/main/java/frc/team555/robot/auto/OldSideAuto.java b/src/main/java/frc/team555/robot/auto/OldSideAuto.java index 98bdffb..52d142f 100644 --- a/src/main/java/frc/team555/robot/auto/OldSideAuto.java +++ b/src/main/java/frc/team555/robot/auto/OldSideAuto.java @@ -61,9 +61,13 @@ public void start() { states.add(new ResetGyro(correction)); states.add(new SetIntakeRotation(intake, intake.downPos)); - if(Side.fromDriverStation()[0] == PowerUpRobot.startSidesChooser.getSelected()){ + if(Side.fromDriverStation()[0] == SwitchAuto.startSidesChooser.getSelected()){ states.add(new DriveEncoderGyro(168, .75, Angle.ZERO, false, correction)); - states.add(new DropCubeSwitch(intake, intakeLift, Side.fromDriverStation()[0], correction)); + states.add(new DropCubeSwitch(new Input(){ + @Override + public Side get() { + return Side.fromDriverStation()[0];} + }, correction)); Debug.msg("Target", "RightSwitch"); }else if(crossover.get()){ states.add(new DriveEncoderGyro(100, .75, Angle.ZERO, false, correction)); diff --git a/src/main/java/frc/team555/robot/auto/SetIntakeLift.java b/src/main/java/frc/team555/robot/auto/SetIntakeLift.java index d784627..8be9d15 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeLift.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeLift.java @@ -1,8 +1,10 @@ package frc.team555.robot.auto; import frc.team555.robot.components.IntakeLift; +import frc.team555.robot.core.Hardware; import org.montclairrobotics.sprocket.states.State; +@Deprecated public class SetIntakeLift implements State { static final double power = .5; @@ -21,22 +23,21 @@ public SetIntakeLift(int position, IntakeLift lift){ @Override public void start() { - lift.setPower(power); - lift.setPosition(position); + // lift.setPower(power); } @Override public void stop() { - + lift.setPower(0); } @Override public void stateUpdate() { - + lift.setPower(power); } @Override public boolean isDone() { - return false; + return Hardware.intakeLiftEncoder.getInches().get() > position; } } diff --git a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java index 2554312..6c5b03a 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java @@ -3,6 +3,7 @@ import frc.team555.robot.components.CubeIntake; import org.montclairrobotics.sprocket.states.State; +@Deprecated public class SetIntakeRotation implements State { int position; CubeIntake intake; @@ -14,7 +15,7 @@ public SetIntakeRotation(CubeIntake intake, int position) { @Override public void start() { - intake.rotationalMotor.setTarget(position); + // intake.roationalMotor.setTarget(position); } @Override diff --git a/src/main/java/frc/team555/robot/auto/SideTurn.java b/src/main/java/frc/team555/robot/auto/SideTurn.java index e8b96a9..5cc44cd 100644 --- a/src/main/java/frc/team555/robot/auto/SideTurn.java +++ b/src/main/java/frc/team555/robot/auto/SideTurn.java @@ -1,28 +1,48 @@ package frc.team555.robot.auto; +import edu.wpi.first.wpilibj.interfaces.Gyro; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.auto.states.TurnGyro; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; +import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Input; public class SideTurn extends TurnGyro { - public Side side; + public Input side; + private boolean relative; + private GyroCorrection gyro; - public SideTurn( GyroCorrection gyro, boolean relative, Side side) { + public SideTurn( GyroCorrection gyro, boolean relative, Input side) { super(Angle.ZERO, gyro, relative); this.side = side; + this.gyro=gyro; + TURN_SPEED=0.25; + } + public void userStart() { + super.userStart(); + if (this.relative) { + this.gyro.setTargetAngleRelative(getTgt()); + } else { + this.gyro.setTargetAngleReset(getTgt()); + } } - @Override public Angle getTgt(){ - if(side == Side.LEFT){ + if(side.get() == Side.LEFT){ return new Degrees(90); }else{ return new Degrees(-90); } } + @Override + public boolean isDone() + { + return super.isDone()||super.timeInState()>5; + } + } diff --git a/src/main/java/frc/team555/robot/auto/SimpleDropCube.java b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java new file mode 100644 index 0000000..ab1a56e --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java @@ -0,0 +1,40 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.components.MainLift; +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.AutoState; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.DriveTime; +import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.states.MultiState; +import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.utils.Input; + + +public class SimpleDropCube extends StateMachine{ + public SimpleDropCube(MainLift mainLift, CubeIntake intake, GyroCorrection correction, Input side){ + super( + new MultiState( + new MoveLift(mainLift,MainLift.TOP*0.5,1,true), + new DriveEncoderGyro(140, .7, Angle.ZERO, false, correction) + ), + new SideTurn(correction, false, side), + // new MainLiftMove(12,.5,true), + new DriveTime(1, .5), + new AutoState() { + @Override + public void stateUpdate() { + intake.setAutoPower(.3); + } + + @Override + public boolean isDone() { + return super.timeInState()>4; + } + } + ); + } +} diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 2371087..8035cbf 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -3,26 +3,35 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; -import frc.team555.robot.components.SimpleIntake; import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.states.Delay; import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.DriveTime; import org.montclairrobotics.sprocket.auto.states.ResetGyro; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; + public class SwitchAuto extends StateMachine{ static Input startSide; + public static SendableChooserstartSidesChooser; public static void init(){ + startSidesChooser = new SendableChooser(); + for(Side side : Side.values()){ + startSidesChooser.addObject(side.toString(), side); + } + //SmartDashboard.putData(startSidesChooser); startSide = new Input() { @Override public Boolean get() { - return Side.fromDriverStation()[0] == PowerUpRobot.startSidesChooser.getSelected(); + return Side.fromDriverStation()[0] == startSidesChooser.getSelected(); } }; } @@ -31,9 +40,27 @@ public static Boolean getStartSide(){ return startSide.get(); } - public SwitchAuto(GyroCorrection correction, CubeIntake intake, MainLift lift){ + public static void disabled(){ + SmartDashboard.putData("Start Position", startSidesChooser); + } + + public static void loop(){ + SmartDashboard.putBoolean("Correct Side", startSide.get()); + } + + public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake){ super(new ResetGyro(correction), - new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)); - // new ConditionalState(new DropCube(intake, lift, PowerUpRobot.startSidesChooser.getSelected(), startSide)); + new ConditionalState(new SimpleDropCube(mainLift,intake, correction, new Input(){ + @Override + public Side get() { + return SwitchAuto.startSidesChooser.getSelected(); + } + }), SwitchAuto.startSide), + new ConditionalState(new DriveEncoderGyro(180,0.5,Angle.ZERO,false,correction),new Input(){ + @Override + public Boolean get() { + return !SwitchAuto.startSide.get(); + } + })); } } diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java index 86aabe6..b1124fb 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java @@ -16,6 +16,7 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; +@Deprecated public class SwitchAuto2 extends StateMachine { static Input startSide; @@ -45,7 +46,7 @@ public SwitchAuto2(GyroCorrection correction){ super(new ResetGyro(correction), new MoveLiftTime(.5,.5), new MoveLiftTime(-.5,.5), - new MoveIntake(-.8,0.5), + //new MoveIntake(-.8,0.5), new MultiState( new MoveLiftTime(.5, 2), new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)), @@ -57,9 +58,10 @@ public static class RotateAndDropCube extends StateMachine { public RotateAndDropCube(GyroCorrection correction) { super( - new SideTurn(correction, false, startSidesChooser.getSelected()), - new DriveTime(3, .3), - new MoveIntake(.8,0.5)); + new SideTurn(correction, false, new Input(){ + public Side get(){return startSidesChooser.getSelected();}}), + new DriveTime(3, .3)); + //new MoveIntake(.8,0.5)); } } @@ -84,7 +86,7 @@ public void userStop() { } } - public static class MoveIntake extends Delay + /*public static class MoveIntake extends Delay { private double power; public MoveIntake(double power,double time) @@ -102,6 +104,6 @@ public void userStop() { Hardware.motorIntakeClamp.set(0); } - } + }*/ } diff --git a/src/main/java/frc/team555/robot/auto/TopCubeAuto.java b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java new file mode 100644 index 0000000..da879bc --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java @@ -0,0 +1,33 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.components.MainLift; +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.utils.Input; + +public class TopCubeAuto extends StateMachine { + public TopCubeAuto(MainLift mainLift, CubeIntake intake, GyroCorrection correction) { + super(new ResetGyro(correction), + //new MoveLift(mainLift, MainLift.TOP*0.6,1,true), + + new ConditionalState(new DropCubeTop(intake, correction, new Input(){ + @Override + public Side get() { + return SwitchAuto.startSidesChooser.getSelected(); + } + }), SwitchAuto.startSide), + new ConditionalState(new DriveEncoderGyro(140,0.4,Angle.ZERO,false,correction),new Input(){ + @Override + public Boolean get() { + return !SwitchAuto.startSide.get(); + } + })); + } +} diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 101a17d..94a0a50 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -1,7 +1,10 @@ package frc.team555.robot.components; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; +import frc.team555.robot.utils.BangBang; +import frc.team555.robot.utils.LimitedMotor; import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; import org.montclairrobotics.sprocket.geometry.Vector; @@ -11,7 +14,7 @@ import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.motors.SEncoder; +import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.Togglable; @@ -19,118 +22,152 @@ public class CubeIntake implements Updatable, Togglable{ - public final Motor left; - public final Motor right; - public final Motor clamp; - public final TargetMotor rotationalMotor; - public final double tolerance = .01; // Todo: Maybe needs to be tuned + private double rotatePowerUp=.5; + private double rotatePowerDown=-.2; + + private boolean auto; + + public Motor left; + public Motor right; + public TargetMotor rotate; + public final double tolerance = 1; // Todo: Maybe needs to be tuned public final int upPos = 0; public final int downPos = 2; public final int middlePos = 1; + private double speed=1; - public final Input power; + public Input power; - private long clampStart=0; - private long clampTime=500; // Todo: needs to be tuned - private double clampPower=0.5; - private boolean clampOpen=true; // True for open, false for close - private double rotationalMotorPower = 0; - private final double roatatePower = .5; - public CubeIntake() { this.left = new Motor(Hardware.motorIntakeL); this.right = new Motor(Hardware.motorIntakeR); - this.clamp = new Motor(Hardware.motorIntakeClamp); - this.rotationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new PID(.1, 0, 0), new Motor(Hardware.motorRotational)); // Todo: needs to be implemented - + right.setInverted(true); + //this.clamp = new Motor(Hardware.motorIntakeClamp); + //LimitedMotor rotateMotor=new LimitedMotor(Hardware.motorIntakeRotate,true,Hardware.intakeLowerBound,Hardware.intakeUpperBound); + Motor rotateMotor=new Motor(Hardware.motorIntakeRotate); + this.rotate=new TargetMotor(Hardware.intakeRotationEncoder,new BangBang(tolerance,rotatePowerUp),rotateMotor); + // this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented + this.power = new Input() { @Override public Vector get() { - return new XY( - -Control.auxStick.getY() + Control.auxStick.getX(), - -Control.auxStick.getY() - Control.auxStick.getX() - ); + double x=-Control.auxStick.getX(); + if(Math.abs(x)<0.1) + { + x=0; + } + double y=-Control.auxStick.getY(); + if(Math.abs(y)<0.1) + { + y=-0.05; + } + return new XY(x,y); } }; - Control.intakeRotationDown.setPressAction(new ButtonAction() { + + + + /*Control.intakeRotateDown.setPressAction(new ButtonAction() { @Override public void onAction() { - rotationalMotor.set(downPos); + rotate.setTarget(downPos); + SmartDashboard.putString("Rotating Down","DOWN"); } }); - Control.intakeRotationUp.setPressAction(new ButtonAction() { + + Control.intakeRotateUp.setPressAction(new ButtonAction() { @Override public void onAction() { - rotationalMotor.set(upPos); + rotate.setTarget(upPos); } }); - Control.intakeRotationMiddle.setPressAction(new ButtonAction() { + Control.intakeRotateMiddle.setPressAction(new ButtonAction() { @Override public void onAction() { - rotationalMotor.set(middlePos); + + rotate.setTarget(middlePos); } }); + */ - Control.intakeLiftManualUp.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(rotationalMotorPower); - } - }); - Control.intakeLiftManualUp.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(rotationalMotorPower); - } - }); - - new CurrentMonitor("Intake Right Motor", Hardware.motorIntakeR, new Input() { + Control.intakeRotateUpManual.setPressAction(new ButtonAction() { @Override - public Boolean get() { - return power.get().getMagnitude() > .1; + public void onAction() { + setPower(rotatePowerUp); } }); - - new CurrentMonitor("Intake Left Motor", Hardware.motorIntakeL, new Input() { + Control.intakeRotateUpManual.setReleaseAction(new ButtonAction() { @Override - public Boolean get() { - return power.get().getMagnitude() > .1; + public void onAction() { + rotate.setPower(0); } }); - new CurrentMonitor("Intake Rotational Motor", Hardware.motorRotational, new Input() { + Control.intakeRotateDownManual.setPressAction(new ButtonAction() { @Override - public Boolean get() { - return Math.abs(rotationalMotor.getTarget() - rotationalMotor.getDistance().get()) > 20; + public void onAction() { + setPower(rotatePowerDown); } - }).setEncoder(Hardware.intakeRotationEncoder); + }); + Control.intakeRotateDownManual.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + rotate.setPower(0); + } + }); + rotate.setPower(0); Updater.add(this, Priority.CALC); } + public void setPower(double pow) { + /*if (pow > 0 && Hardware.intakeUpperBound.get()) //Upper limit switch is not present + { + pow=0; + } + else*/ if (pow < 0 && !Hardware.intakeLowerBound.get()) //Lower Limit switch is inverted + { + pow=0; + } + rotate.setPower(pow); + } + @Override public void update() { Vector p = power.get(); - // if(p.getMagnitude() < tolerance){ - // openClamp(); - // }else{ - // closeClamp(); - // } - left.set(p.getX()); - right.set(p.getY()); - - // updateClamp(); + + if(!auto) { + left.set((p.getY() - p.getX()*.25)*speed); + right.set((p.getY() + p.getX()*.25)*speed); + } + auto=false; + Debug.msg("RotateMotor PID OUT",rotate.pid.get()); + Debug.msg("RotateMotor PID TGT",rotate.pid.getTarget()); + Debug.msg("RotateMotor Encoder",rotate.pid.getCurInput()); + } + + + public void auto() + { + auto=true; + enable(); + } + + public void setAutoPower(double power){ + auto = true; + left.set(power); + right.set(power); } @Override public void enable() { - left.set(-.5); - right.set(-.5); + left.set(1); + right.set(1); } @Override @@ -139,49 +176,4 @@ public void disable() { right.set(0); } - private void openClamp(){ - if(!clampOpen) - { - clampOpen=true; - startClamp(); - } - } - - private void closeClamp(){ - if(clampOpen) - { - clampOpen=false; - startClamp(); - } - } - - private void startClamp() - { - //Guess how far we have moved, and set our start time correctly - //clampStart=System.currentTimeMillis()-(clampTime-(System.currentTimeMillis()-clampStart))); - clampStart=System.currentTimeMillis()*2-clampTime-clampStart; - if(clampStart>System.currentTimeMillis()) - { - clampStart=System.currentTimeMillis(); - } - } - - private void updateClamp() - { - if(System.currentTimeMillis()-clampStart() { - @Override - public Boolean get() { - return Control.intakeLiftManualUp.get() || Control.intakeLiftManualUp.get() || Math.abs(motors.getTarget() - motors.getDistance().get()) > 20; - } - }).setEncoder(Hardware.intakeLiftEncoder); + Control.intakeLiftManualUp.setReleaseAction(stop); + Control.intakeLiftManualDown.setReleaseAction(stop); + motors.setPower(0); } public void setPosition(int p) { p = (int)Utils.constrain(p, 0, positions.length - 1); - motors.setTarget(positions[p]); + //motors.setTarget(positions[p]); pos = p; } public void setPositionRaw(double p) { - motors.setTarget(p); + //motors.setTarget(p); + } + + + @Override + public SEncoder getEncoder() { + return encoder; } + @Override + public void setAuto(double power) { + auto=true; + setPower(power); + } public void setPower(double power){ motors.set(power); diff --git a/src/main/java/frc/team555/robot/components/Lift.java b/src/main/java/frc/team555/robot/components/Lift.java new file mode 100644 index 0000000..8e37ce9 --- /dev/null +++ b/src/main/java/frc/team555/robot/components/Lift.java @@ -0,0 +1,8 @@ +package frc.team555.robot.components; + +import org.montclairrobotics.sprocket.motors.SEncoder; + +public interface Lift { + public SEncoder getEncoder(); + public void setAuto(double power); +} diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index cf75c9e..18e58d5 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -1,93 +1,150 @@ package frc.team555.robot.components; + import edu.wpi.first.wpilibj.DigitalInput; +import frc.team555.robot.auto.MoveLift; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; -import frc.team555.robot.utils.MotorMonitor; +import frc.team555.robot.core.PowerUpRobot; +import frc.team555.robot.utils.BangBang; import frc.team555.robot.utils.TargetMotor; +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.DriveTime; import org.montclairrobotics.sprocket.control.ButtonAction; -import org.montclairrobotics.sprocket.motors.CurrentMonitor; + +import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.utils.Input; +import org.montclairrobotics.sprocket.motors.SEncoder; +import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.PID; -public class MainLift extends TargetMotor { +import java.nio.channels.Pipe; + +public class MainLift extends TargetMotor implements Lift { + + private final double speed =1; + public static final double TOP= 30834.0; + private final boolean LIMIT_SWITCH_DISABLED=false; + + private boolean auto=false; - private final double speed = 1.0; + public boolean hasReset=false; private int upPosition; private int downPosition; private DigitalInput bottomLimitSwitch; public MainLift(){ - super(Hardware.liftEncoder, new PID(.1, 0, 0), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); + + super(Hardware.liftEncoder, new BangBang(1,1), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); mode = Mode.POWER; // Manual Up - Control.mainLiftManualUp.setPressAction(new ButtonAction() { +// Control.mainLiftManualUp.setPressAction(new ButtonAction() { +// @Override +// public void onAction() { +// set(speed); +// } +// }); + +// Control.mainLiftManualUp.setReleaseAction(new ButtonAction() { +// @Override +// public void onAction() { +// set(0); +// } +// }); + + // Auto Safe Down + Control.mainLiftAutoDown.setHeldAction(new ButtonAction() { @Override public void onAction() { - MainLift.super.set(speed); + if((LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) + //&&Control.auxStick.getRawButton(1) + ) { + if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { + set(-speed); + } + else { + set(-0.4); + } + }else{ + set(0); + encoder.reset(); + hasReset=true; + } } }); - Control.mainLiftManualUp.setReleaseAction(new ButtonAction() { + + + Control.mainLiftAutoDown.setReleaseAction(new ButtonAction() { @Override public void onAction() { - MainLift.super.set(0); + set(0); } }); - // Manual Down - Control.mainLiftManualDown.setPressAction(new ButtonAction() { + // Lift Top + /* Control.mainLiftTop.setPressAction(new ButtonAction() { @Override public void onAction() { - if(!bottomLimitSwitch.get()) { - MainLift.super.set(-speed); - }else{ - MainLift.super.set(0); - Hardware.liftEncoder.reset(); + if(encoder != null){ + MainLift.super.setTarget(downPosition); } } - }); - - - - Control.mainLiftManualDown.setReleaseAction(new ButtonAction() { + });*/ + /*Control.mainLiftAutoUp.setHeldAction(new ButtonAction() { @Override public void onAction() { - if(encoder != null) { - MainLift.super.set(0); + if(Hardware.liftEncoder.getInches().get()() { + Control.mainLiftManualUp.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + Hardware.motorLiftMainFront.set(0); + Hardware.motorLiftMainBack.set(0); + } + }); + + /*Control.mainLiftAutoUp.setReleaseAction(new ButtonAction() { @Override - public Boolean get() { - return Math.abs(getTarget() - getDistance().get()) > 20 || speed != 0; + public void onAction() { + Hardware.motorLiftMainFront.set(0); + Hardware.motorLiftMainBack.set(0); } - }).setEncoder(Hardware.liftEncoder); + });*/ + setPower(0); - new CurrentMonitor("Main Lift Back", Hardware.motorLiftMainBack, new Input() { - @Override - public Boolean get() { - return Math.abs(getTarget() - getDistance().get()) > 20 || speed != 0; - } - }).setEncoder(Hardware.liftEncoder); // Lift Bottom @@ -97,5 +154,14 @@ public Boolean get() { } + @Override + public SEncoder getEncoder() { + return encoder; + } + @Override + public void setAuto(double power) { + setPower(power); + auto=true; + } } diff --git a/src/main/java/frc/team555/robot/components/SimpleIntake.java b/src/main/java/frc/team555/robot/components/SimpleIntake.java deleted file mode 100644 index 23cbbf7..0000000 --- a/src/main/java/frc/team555/robot/components/SimpleIntake.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc.team555.robot.components; - -import frc.team555.robot.auto.ConditionalState; -import frc.team555.robot.core.Control; -import frc.team555.robot.core.Hardware; -import org.montclairrobotics.sprocket.control.Button; -import org.montclairrobotics.sprocket.control.ButtonAction; -import org.montclairrobotics.sprocket.control.JoystickButton; -import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.geometry.XY; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.Togglable; - - -public class SimpleIntake { - - public final Motor clamp; - - public SimpleIntake() { - this.clamp = new Motor(Hardware.motorIntakeClamp); - - - Control.openButton.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - clamp.set(1); - } - }); - Control.openButton.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - clamp.set(0); - } - }); - - Control.closeButton.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - clamp.set(-1); - } - }); - Control.closeButton.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - clamp.set(0); - } - }); - - //Updater.add(this, Priority.CALC); - } - -/* - @Override - public void update() { - double p = - Vector p = power.get(); - if(p.getMagnitude() < tolerance){ - openClamp(); - }else{ - closeClamp(); - } - left.set(p.getX()); - right.set(p.getY()); - updateClamp(); - } - - @Override - public void enable() { - left.set(-.5); - right.set(-.5); - } - - @Override - public void disable() { - left.set(0); - right.set(0); - } - - private void openClamp(){ - if(!clampOpen) - { - clampOpen=true; - startClamp(); - } - } - - private void closeClamp(){ - if(clampOpen) - { - clampOpen=false; - startClamp(); - } - } - - private void startClamp() - { - //Guess how far we have moved, and set our start time correctly - //clampStart=System.currentTimeMillis()-(clampTime-(System.currentTimeMillis()-clampStart))); - clampStart=System.currentTimeMillis()*2-clampTime-clampStart; - if(clampStart>System.currentTimeMillis()) - { - clampStart=System.currentTimeMillis(); - } - } - - private void updateClamp() - { - if(System.currentTimeMillis()-clampStart startSidesChooser; + public static CubeIntake intake; + MainLift mainLift; + IntakeLift intakeLift; + StateMachine autoClimb; + //SendableChooser startSideChooser; //vision stuff private static final int IMG_WIDTH = 320; private static final int IMG_HEIGHT = 240; + private static final double fullSpeed=0.4; + private static final double halfSpeed=0.6; private static final double oldOverNew=17.1859 * 1.25/(6544.0/143.0); @@ -52,7 +62,18 @@ public class PowerUpRobot extends SprocketRobot { @Override public void robotInit(){ + //startSideChooser = new SendableChooser<>(); + //startSideChooser.addObject("Right", Side.RIGHT); + //startSideChooser.addObject("Right", Side.LEFT); + /*switchOnSide = new Input() { + @Override + public Boolean get() { + return Side.fromDriverStation()[0] == startSide; + } + };*/ + + CameraServer.getInstance().startAutomaticCapture(); DriveEncoders.TOLLERANCE=/*45.5363/17.1859*/6; TurnGyro.TURN_SPEED=0.3; TurnGyro.tolerance=new Degrees(3); @@ -62,22 +83,27 @@ public void robotInit(){ SwitchAuto.init(); DriveModule[] modules = new DriveModule[2]; intake = new CubeIntake(); + mainLift=new MainLift(); + intakeLift=new IntakeLift(); + correction = new GyroCorrection(Hardware.navx, new PID(1.5, 0, 0.0015), 90, 1); + autoClimb = new AutoClimbSequence(mainLift); + //SetIntakeLift.setLift(intakeLift); modules[0] = new DriveModule(new XY(-1, 0), new XY(0, 1), Hardware.leftDriveEncoder, new PID(0.5,0,0), Module.MotorInputType.PERCENT, - new Motor(Hardware.motorDriveBL), - new Motor(Hardware.motorDriveFL)); + new CoastMotor(Hardware.motorDriveBL,false), + new CoastMotor(Hardware.motorDriveFL,false)); modules[1] = new DriveModule(new XY(1, 0), new XY(0, 1), Hardware.rightDriveEncoder, new PID(0.5,0,0), Module.MotorInputType.PERCENT, - new Motor(Hardware.motorDriveBR), - new Motor(Hardware.motorDriveFR)); + new CoastMotor(Hardware.motorDriveBR,false), + new CoastMotor(Hardware.motorDriveFR,false)); new CurrentMonitor("Front Right Drive Motor", Hardware.motorDriveFR, new Input() { @Override @@ -129,15 +155,41 @@ public Boolean get() { /* Drive Train Pipeline: GyroCorrection, Deadzone */ + + //VISION + DTStep vision =new DTStep() { + @Override + public DTTarget get(DTTarget dtTarget) { + if(!Control.driveStick.getRawButton(7)) + { + return dtTarget; + } + double turn=0; + double x=SmartDashboard.getNumber("cubeX",600)-600; + if(x<-100) + { + turn=-.1; + } + else if(x>100) + { + turn=.1; + } + return new DTTarget(dtTarget.getDirection(),new Radians(turn)); + } + }; + new DashboardInput("auto Selection"); - ArrayList> steps = new ArrayList<>(); - correction = new GyroCorrection(Hardware.navx, new PID(1.5, 0, 0.0015), 90, 1); + + + ArrayList> steps = new ArrayList<>(); + sensitivity=new Sensitivity(fullSpeed,fullSpeed*0.8); lock = new GyroLock(correction); steps.add(new Deadzone()); - steps.add(new Sensitivity(1)); + steps.add(sensitivity); steps.add(correction); + steps.add(vision); driveTrain.setPipeline(new DTPipeline(steps)); /* Enabling and Disabling GyroLock */ @@ -161,17 +213,35 @@ public void onAction() { /*super.addAutoMode(new AutoMode("Dynamic auto", new DynamicAutoState())); new DriveEncoderGyro(12*30,.5,new Degrees(0),false,correction); */ - Togglable fieldInput = new FieldCentricDriveInput(Control.driveStick,correction); - new ToggleButton(Control.driveStick,Control.FieldCentricID,fieldInput); + //Togglable fieldInput = new FieldCentricDriveInput(Control.driveStick,correction); + //new ToggleButton(Control.driveStick,Control.FieldCentricID,fieldInput); - Button resetButton=new JoystickButton(Control.driveStick,Control.ResetID); - resetButton.setPressAction(new ButtonAction() { + //Button resetButton=new JoystickButton(Control.driveStick,Control.ResetID); + /*resetButton.setPressAction(new ButtonAction() { @Override public void onAction() { correction.reset(); } + });*/ + + /*Control.halfSpeed.setPressAction(new ButtonAction() { + @Override + public void onAction() { + sensitivity.set(0.5,0.3); + } }); + Control.halfSpeed.setReleaseAction(new ButtonAction() { + Control.halfSpeed.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + sensitivity.set(1,0.6); + } + }); +*/ + + + //auto final double driveSpeed = 0.4; @@ -256,15 +326,32 @@ public void onAction() { new ResetGyro(correction), new DriveEncoderGyro(-12*10,.5,new Degrees(0),false,correction)); + + AutoMode mainLiftUp= new AutoMode("Main Lift Up",new MoveLift(mainLift,MainLift.TOP*0.5,1,true)); + AutoMode turnQuarter=new AutoMode("Turn Quarter",new TurnGyro(Angle.QUARTER,correction,true)); + + // addAutoMode(new AutoClimbSequence(mainLift)); + addAutoMode(autoDrive); addAutoMode(baseLine); addAutoMode(centerBaseLineLeft); addAutoMode(centerBaseLineRight); + + /* Joshua Rapoport: AutoSwitch (left, right, middle) */ + + // addAutoMode(AutoSwitch.fromSide(Side.LEFT)); + // addAutoMode(AutoSwitch.fromSide(Side.RIGHT)); + // addAutoMode(AutoSwitch.fromMiddle()); + addAutoMode(new AutoMode("Switch Using Intake", new SwitchAuto(mainLift,correction, intake))); + addAutoMode(mainLiftUp); + addAutoMode(turnQuarter); + addAutoMode(new AutoMode("Switch Using Lift", new TopCubeAuto(mainLift, intake, correction))); + addAutoMode(new AutoMode("Drive 120 Inches",new DriveEncoderGyro(120,.5,Angle.ZERO,true,correction))); //addAutoMode(new AutoMode("Switch Auto", new SwitchAuto(correction, intake))); sendAutoModes(); - StateMachine shootCube = new StateMachine(false, new SetIntakeRotation(intake, intake.middlePos), new CubeOuttake(intake, 1), new SetIntakeRotation(intake, intake.downPos)); + //StateMachine shootCube = new StateMachine(false, new SetIntakeRotation(intake, intake.middlePos), new CubeOuttake(intake, 1), new SetIntakeRotation(intake, intake.downPos)); - Control.intakeSubroutine.setHeldAction(new ButtonAction() { + /*Control.intakeSubroutine.setHeldAction(new ButtonAction() { @Override public void onAction() { shootCube.start(); @@ -287,9 +374,34 @@ public void onAction() { intake.rotationalMotor.set(intake.downPos); } }); +*/ + +/* + Control.autoClimb.setPressAction(new ButtonAction() { + @Override + public void onAction() { + autoClimb.start(); + } + }); + + Control.autoClimb.setHeldAction(new ButtonAction() { + @Override + public void onAction() { + //autoClimb.stateUpdate(); + } + }); + + Control.autoClimb.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + autoClimb.stop(); + } + }); +*/ // vision stuff +// CameraServer.getInstance().startAutomaticCapture(); /*UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); camera.setResolution(IMG_WIDTH, IMG_HEIGHT); @@ -303,12 +415,6 @@ public void onAction() { }); visionThread.start();*/ - startSidesChooser = new SendableChooser<>(); - for(Side side : Side.values()){ - startSidesChooser.addObject(side.toString(), side); - } - SmartDashboard.putData(startSidesChooser); - } @Override @@ -327,13 +433,26 @@ public void update(){ SmartDashboard.putNumber("turn", turn); */ + + double cubeCenterX=SmartDashboard.getNumber("cubeCenterX",0.0); + SmartDashboard.putNumber("x rebroadcasted",cubeCenterX); SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); Debug.msg("Intake Rotation", Hardware.intakeRotationEncoder.get()); - + SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); + SmartDashboard.putNumber("POV",Control.auxStick.getPOV()); + debugCurrent("Main Lift Front",Hardware.motorLiftMainFront); + debugCurrent("Main Lift Back",Hardware.motorLiftMainBack); + debugCurrent("Intake Lift",Hardware.motorLiftIntake); + SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); + SmartDashboard.putNumber("Intake Lift Encoder",Hardware.motorLiftIntake.getSensorCollection().getQuadraturePosition()); gyroLocking(); + //startSide = startSideChooser.getSelected(); + } + private void debugCurrent(String name,WPI_TalonSRX motor) { + SmartDashboard.putNumber(name + " Current", motor.getOutputCurrent()); } private void gyroLocking(){ @@ -345,10 +464,20 @@ private void gyroLocking(){ lock.disable(); } //lock.update(); + if(Hardware.liftEncoder.getInches().get()>MainLift.TOP*0.5&&!Control.driveStick.getRawButton(3)||Control.driveStick.getRawButton(2)) + { + sensitivity.set(halfSpeed,0.8*fullSpeed); + } + else + { + sensitivity.set(fullSpeed,0.8*fullSpeed); + } } @Override public void userDisabledPeriodic(){ - SmartDashboard.putData(startSidesChooser) ; + //SmartDashboard.putData(startSideChooser); + //startSide = startSideChooser.getSelected(); + SwitchAuto.disabled(); } } diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot2.java b/src/main/java/frc/team555/robot/core/PowerUpRobot2.java deleted file mode 100644 index d6ba743..0000000 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot2.java +++ /dev/null @@ -1,502 +0,0 @@ -package frc.team555.robot.core; - -import edu.wpi.first.wpilibj.CameraServer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.team555.robot.auto.SwitchAuto; -import frc.team555.robot.auto.SwitchAuto2; -import frc.team555.robot.components.CubeIntake; -import frc.team555.robot.components.MainLift; -import frc.team555.robot.components.SimpleIntake; -import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.auto.states.*; -import org.montclairrobotics.sprocket.control.*; -import org.montclairrobotics.sprocket.drive.*; -import org.montclairrobotics.sprocket.drive.steps.AccelLimit; -import org.montclairrobotics.sprocket.drive.steps.Deadzone; -import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; -import org.montclairrobotics.sprocket.drive.steps.Sensitivity; -import org.montclairrobotics.sprocket.drive.utils.GyroLock; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.XY; -import org.montclairrobotics.sprocket.motors.Module; -import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.Togglable; - -import java.awt.image.renderable.ContextualRenderedImageFactory; -import java.util.ArrayList; - -public class PowerUpRobot2 extends SprocketRobot { - public static final boolean SWITCHES=false; - - - DriveTrain driveTrain; - GyroCorrection correction; - GyroLock lock; - boolean manualLock; - //SimpleIntake intake; - //MainLift lift; - Sensitivity sensitivity; - - private double dir=1.0; - private AccelLimit limit; - - public static final double sD=1,sT=.7;//sensitivity Direction and Turn - - //vision stuff - private static final int IMG_WIDTH = 320; - private static final int IMG_HEIGHT = 240; - - - private static final double oldOverNew=17.1859 * 1.25/(6544.0/143.0); - - //private double centerX = 0.0; - //private VisionThread visionThread; - - //private final Object imgLock = new Object(); - - @Override - public void robotInit(){ - - - - - DriveEncoders.TOLLERANCE=/*45.5363/17.1859*/12; - TurnGyro.TURN_SPEED=0.3; - TurnGyro.tolerance=new Degrees(3); - //40 ft 5.5 in - Hardware.init(); - Control.init(); - SwitchAuto2.init(); - DriveModule[] modules = new DriveModule[2]; - //intake = new SimpleIntake(); - //lift=new MainLift(); - limit=new AccelLimit(3,5); - - - //Temp Controls - //Lift - Input liftInput=new JoystickYAxis(Control.auxStick); - ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorLiftMainBack, liftInput); - ControlledMotor testMotor3 = new ControlledMotor(Hardware.motorLiftMainFront, liftInput); - - //Intake - Button intakeLiftUp=new JoystickButton(Control.auxStick,3); - Button intakeLiftDown=new JoystickButton(Control.auxStick,2); - intakeLiftUp.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftIntake.set(1); - } - }); - intakeLiftDown.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftIntake.set(-1); - } - }); - ButtonAction stopIntakeLift=new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftIntake.set(0); - } - }; - intakeLiftUp.setReleaseAction(stopIntakeLift); - intakeLiftDown.setReleaseAction(stopIntakeLift); - - //Clamp - Button clampOut=new JoystickButton(Control.auxStick,5); - Button clampIn=new JoystickButton(Control.auxStick,4); - clampOut.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - if(SWITCHES && Hardware.intakeOpenSwitch.get()) { - Hardware.motorIntakeClamp.set(0); - } - else - { - Hardware.motorIntakeClamp.set(1); - } - } - }); - clampIn.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - if(SWITCHES && Hardware.intakeOpenSwitch.get()) { - Hardware.motorIntakeClamp.set(0); - } - else - { - Hardware.motorIntakeClamp.set(-1); - } - } - }); - ButtonAction stopClamp=new ButtonAction() { - @Override - public void onAction() { - Hardware.motorIntakeClamp.set(0); - } - }; - clampOut.setReleaseAction(stopClamp); - clampIn.setReleaseAction(stopClamp); - - - //Auto lift - JoystickButton autoLift=new JoystickButton(Control.auxStick,1); - autoLift.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - if(Hardware.liftEncoder.getInches().get()<31600) - { - Hardware.motorLiftMainFront.set(1); - Hardware.motorLiftMainBack.set(1); - } - else - { - Hardware.motorLiftMainFront.set(0); - Hardware.motorLiftMainBack.set(0); - } - testMotor2.disable(); - testMotor3.disable(); - } - }); - autoLift.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftMainFront.set(0); - Hardware.motorLiftMainBack.set(0); - - testMotor2.enable(); - testMotor3.enable(); - } - }); - - - //Half Speed - Button halfSpeedButton=new JoystickButton(Control.auxStick,4); - halfSpeedButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - sensitivity.set(sD*0.5*dir, sT*0.5); - } - }); - - halfSpeedButton.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - sensitivity.set(sD*dir, sT); - } - }); - - //Backwards Button - Button backwardsButton=new JoystickButton(Control.driveStick,12); - backwardsButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - dir=-1; - } - }); - - //Forwards Button - Button forwardsButton=new JoystickButton(Control.driveStick,11); - backwardsButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - dir=1; - } - }); - - //Limiter Off - Button accelLimitOff=new JoystickButton(Control.driveStick,7); - accelLimitOff.setPressAction(new ButtonAction() { - @Override - public void onAction() { - limit.disable(); - } - }); - - //Limiter Off - Button accelLimitOn=new JoystickButton(Control.driveStick,7); - accelLimitOn.setPressAction(new ButtonAction() { - @Override - public void onAction() { - limit.enable(); - } - }); - Motor mBL=new Motor(Hardware.motorDriveBL, false); - Motor mBR=new Motor(Hardware.motorDriveBR, false); - Motor mFL=new Motor(Hardware.motorDriveFL, false); - Motor mFR=new Motor(Hardware.motorDriveFR, false); - - - modules[0] = new DriveModule(new XY(-1, 0), - new XY(0, 1), - Hardware.leftDriveEncoder, - new PID(0.5,0,0), - Module.MotorInputType.PERCENT, - new Motor(Hardware.motorDriveBL), - new Motor(Hardware.motorDriveFL)); - - modules[1] = new DriveModule(new XY(1, 0), - new XY(0, 1), - Hardware.rightDriveEncoder, - new PID(0.5,0,0), - Module.MotorInputType.PERCENT, - new Motor(Hardware.motorDriveBR), - new Motor(Hardware.motorDriveFR)); - - - - DriveTrainBuilder dtBuilder = new DriveTrainBuilder(); - dtBuilder.addDriveModule(modules[0]).addDriveModule(modules[1]); - - /* Build Drive Train */ - - dtBuilder.setInput(Control.driveInput); - dtBuilder.setDriveTrainType(DriveTrainType.TANK); - try { - this.driveTrain = dtBuilder.build(); - } catch (InvalidDriveTrainException e) { - e.printStackTrace(); - } - - /* Drive Train Configurations: Tank, Control */ - - driveTrain.setMapper(new TankMapper()); - driveTrain.setDefaultInput(Control.driveInput); - - /* Drive Train Pipeline: GyroCorrection, Deadzone */ - - - new DashboardInput("auto Selection"); - - ArrayList> steps = new ArrayList<>(); - - correction = new GyroCorrection(Hardware.navx, new PID(1.5, 0, 0.0015), 90, 1); - lock = new GyroLock(correction); - sensitivity=new Sensitivity(sD*dir,sT); - steps.add(new Deadzone()); - steps.add(limit); - steps.add(sensitivity); - steps.add(correction); - driveTrain.setPipeline(new DTPipeline(steps)); - - /* Enabling and Disabling GyroLock */ - - Control.lock.setPressAction(new ButtonAction() { - @Override - public void onAction() { - manualLock = true; - } - }); - - Control.lock.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - manualLock = false; - } - }); - //this.intake = - //new CubeIntake(); - - - - /*super.addAutoMode(new AutoMode("Dynamic auto", new DynamicAutoState())); -new DriveEncoderGyro(12*30,.5,new Degrees(0),false,correction); -*/ - Togglable fieldInput = new FieldCentricDriveInput(Control.driveStick,correction); - new ToggleButton(Control.driveStick,Control.FieldCentricID,fieldInput); - - Button resetButton=new JoystickButton(Control.driveStick,Control.ResetID); - resetButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - correction.reset(); - } - }); - - - //auto - final double driveSpeed = 0.4; - final int maxEncAccel = 10; - final int maxTicksPerSec = 10; - /*AutoMode autoDrive = new AutoMode("auto Drive", - new DriveEncoderGyro(120, - 0.25, - Angle.ZERO, - true, - correction)); - - - AutoMode encoder = new AutoMode("encoder", - new DriveEncoders(100,.25)); - - AutoMode turn90 = new AutoMode("Turn 90", - new TurnGyro(new Degrees(90),correction,true)); - - AutoMode square = new AutoMode("Square", - new ResetGyro(correction), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(90),correction,false), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(180),correction,false), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(270),correction,false), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(0),correction,false)); - - AutoMode square2 = new AutoMode("Square 2", - new ResetGyro(correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(0),false,correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(90),false,correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(180),false,correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(270),false,correction)); - -*/ - AutoMode baseLine = new AutoMode("Base Line", - new ResetGyro(correction), - new DriveEncoderGyro(150, .5 , new Degrees(0), false, correction)); - AutoMode centerBaseLineRight = new AutoMode("Center Base Line Right", - new ResetGyro(correction), - new DriveEncoderGyro(60, .5, new Degrees(0), false, correction), - new DriveEncoderGyro(12, .5, new Degrees(90), false, correction), - new DriveEncoderGyro(80, .5, new Degrees(0), false, correction)); - AutoMode centerBaseLineLeft = new AutoMode("Center Base Line Left", - new ResetGyro(correction), - new DriveEncoderGyro(60, .5, new Degrees(0), false, correction), - new DriveEncoderGyro(122, .5, new Degrees(-90), false, correction), - new DriveEncoderGyro(80, .5, new Degrees(0), false, correction)); - - AutoMode switchAuto2=new AutoMode("Switch",new SwitchAuto2(correction)); - -/* - AutoMode twentyFeet=new AutoMode("Twenty Feet", - new ResetGyro(correction), - new DriveEncoderGyro(12*20,.5,new Degrees(0),false,correction)); - addAutoMode(twentyFeet); - - AutoMode tenFeet=new AutoMode("ten Feet", - new ResetGyro(correction), - new DriveEncoderGyro(12*10,.5,new Degrees(0),false,correction)); - addAutoMode(tenFeet); - - AutoMode thirtyFeet=new AutoMode("thirty Feet", - new ResetGyro(correction), - new DriveEncoderGyro(12*30,.5,new Degrees(0),false,correction)); - addAutoMode(thirtyFeet); - AutoMode farAuto = new AutoMode("Rich Mode", - new ResetGyro(correction), - new DriveEncoderGyro(12*4*oldOverNew, .5,new Degrees(0),false,correction), - new DriveEncoderGyro((12*20+4)*oldOverNew,.5,new Degrees(90),false,correction), - new DriveEncoderGyro((12*4)*oldOverNew, .5,new Degrees(180),false,correction), - new Delay(5), - new DriveEncoderGyro((-12*4)*oldOverNew, .5,new Degrees(180),false,correction), - new DriveEncoderGyro((-12*20-4)*oldOverNew,.5,new Degrees(90),false,correction), - new DriveEncoderGyro((-12*4)*oldOverNew, .5,new Degrees(0),false,correction)); - - AutoMode backTen = new AutoMode("Back Ten", - new ResetGyro(correction), - new DriveEncoderGyro(-12*10,.5,new Degrees(0),false,correction)); - */ - addAutoMode(new AutoMode("Drive 3 Feet",new DriveEncoderGyro(3*12,.3,Angle.ZERO,true,correction))); - addAutoMode(new AutoMode("nothing", new Delay(1))); - addAutoMode(baseLine); - addAutoMode(centerBaseLineLeft); - addAutoMode(centerBaseLineRight); - //addAutoMode(new AutoMode("Switch Auto", new SwitchAuto(correction, intake))); - addAutoMode(switchAuto2); - addAutoMode(new AutoMode("Drive Time",new DriveTime(4,.5))); - sendAutoModes(); - - CameraServer.getInstance().startAutomaticCapture(); - - - // vision stuff - /*UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); - camera.setResolution(IMG_WIDTH, IMG_HEIGHT); - -6gtyj visionThread = new VisionThread(camera, new DrivePipeline(), pipeline -> { - if (!pipeline.filterContoursOutput().isEmpty()) { - org.opencv.core.Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0)); - synchronized (imgLock) { - centerX = r.x + (r.width / 2); - } - } - }); - visionThread.start();*/ - - -/* - AutoMode driveBack20Mode=new AutoMode("DriveBack20",new DriveEncoderGyro(-23,0.5,Angle.ZERO,true,correction)); - - JoystickButton driveBack20=new JoystickButton(Control.driveStick,4); - driveBack20.setPressAction(new ButtonAction() { - @Override - public void onAction() { - driveBack20Mode.start(); - } - }); - driveBack20.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - driveBack20Mode.stop(); - } - }); -*/ - } - - @Override - public void userAutonomousSetup(){ - - } - - - @Override - public void update(){ - /*double centerX; - synchronized (imgLock) { - centerX = this.centerX; - } - double turn = centerX - (IMG_WIDTH / 2); - - SmartDashboard.putNumber("turn", turn); -*/ - SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); - SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); - SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); - SmartDashboard.putNumber("Intake Lift Encoder", Hardware.intakeLiftEncoder.getInches().get()); - SmartDashboard.putNumber("Lift",Hardware.liftEncoder.getInches().get()); - if(SWITCHES) { - SmartDashboard.putBoolean("Intake Open", Hardware.intakeOpenSwitch.get()); - SmartDashboard.putBoolean("Intake Closed", Hardware.intakeClosedSwitch.get()); - } - Debug.msg("Intake Position",Hardware.motorLiftIntake.getSensorCollection().getQuadraturePosition()); - Debug.msg("Intake Velocity",Hardware.motorLiftIntake.getSensorCollection().getQuadraturePosition()); - Hardware.motorLiftIntake.getSensorCollection().getQuadratureVelocity(); - - - gyroLocking(); - - } - - private void gyroLocking(){ - boolean autoLock = ((Math.abs(Control.driveInput.getTurn().toDegrees())<10) && - (Math.abs(Control.driveInput.getDir().getY())>0.5)); - if(autoLock || manualLock){ - lock.enable(); - }else{ - lock.disable(); - } - //lock.update(); - } - - @Override - public void userDisabledPeriodic(){ - SwitchAuto2.disabled(); - } -} diff --git a/src/main/java/frc/team555/robot/test/PIDLoopTest.java b/src/main/java/frc/team555/robot/test/PIDLoopTest.java deleted file mode 100644 index 0308568..0000000 --- a/src/main/java/frc/team555/robot/test/PIDLoopTest.java +++ /dev/null @@ -1,500 +0,0 @@ -package frc.team555.robot.core; - -import edu.wpi.first.wpilibj.CameraServer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.team555.robot.auto.SwitchAuto; -import frc.team555.robot.auto.SwitchAuto2; -import frc.team555.robot.components.CubeIntake; -import frc.team555.robot.components.MainLift; -import frc.team555.robot.components.SimpleIntake; -import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.auto.states.*; -import org.montclairrobotics.sprocket.control.*; -import org.montclairrobotics.sprocket.drive.*; -import org.montclairrobotics.sprocket.drive.steps.AccelLimit; -import org.montclairrobotics.sprocket.drive.steps.Deadzone; -import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; -import org.montclairrobotics.sprocket.drive.steps.Sensitivity; -import org.montclairrobotics.sprocket.drive.utils.GyroLock; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.XY; -import org.montclairrobotics.sprocket.motors.Module; -import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.Togglable; - -import java.awt.image.renderable.ContextualRenderedImageFactory; -import java.util.ArrayList; - -public class PIDLoopTest extends SprocketRobot { - public static final boolean SWITCHES=false; - - - DriveTrain driveTrain; - GyroCorrection correction; - GyroLock lock; - boolean manualLock; - //SimpleIntake intake; - //MainLift lift; - Sensitivity sensitivity; - - private double dir=1.0; - private AccelLimit limit; - - public static final double sD=1,sT=.7;//sensitivity Direction and Turn - - //vision stuff - private static final int IMG_WIDTH = 320; - private static final int IMG_HEIGHT = 240; - - - private static final double oldOverNew=17.1859 * 1.25/(6544.0/143.0); - - //private double centerX = 0.0; - //private VisionThread visionThread; - - //private final Object imgLock = new Object(); - - @Override - public void robotInit(){ - - - - - DriveEncoders.TOLLERANCE=/*45.5363/17.1859*/6; - TurnGyro.TURN_SPEED=0.3; - TurnGyro.tolerance=new Degrees(3); - //40 ft 5.5 in - Hardware.init(); - Control.init(); - SwitchAuto2.init(); - DriveModule[] modules = new DriveModule[2]; - //intake = new SimpleIntake(); - //lift=new MainLift(); - limit=new AccelLimit(3,5); - - - //Temp Controls - //Lift - Input liftInput=new JoystickYAxis(Control.auxStick); - ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorLiftMainBack, liftInput); - ControlledMotor testMotor3 = new ControlledMotor(Hardware.motorLiftMainFront, liftInput); - - //Intake - Button intakeLiftUp=new JoystickButton(Control.auxStick,3); - Button intakeLiftDown=new JoystickButton(Control.auxStick,2); - intakeLiftUp.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftIntake.set(1); - } - }); - intakeLiftDown.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftIntake.set(-1); - } - }); - ButtonAction stopIntakeLift=new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftIntake.set(0); - } - }; - intakeLiftUp.setReleaseAction(stopIntakeLift); - intakeLiftDown.setReleaseAction(stopIntakeLift); - - //Clamp - Button clampOut=new JoystickButton(Control.auxStick,5); - Button clampIn=new JoystickButton(Control.auxStick,4); - clampOut.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - if(SWITCHES && Hardware.intakeOpenSwitch.get()) { - Hardware.motorIntakeClamp.set(0); - } - else - { - Hardware.motorIntakeClamp.set(1); - } - } - }); - clampIn.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - if(SWITCHES && Hardware.intakeOpenSwitch.get()) { - Hardware.motorIntakeClamp.set(0); - } - else - { - Hardware.motorIntakeClamp.set(-1); - } - } - }); - ButtonAction stopClamp=new ButtonAction() { - @Override - public void onAction() { - Hardware.motorIntakeClamp.set(0); - } - }; - clampOut.setReleaseAction(stopClamp); - clampIn.setReleaseAction(stopClamp); - - - //Auto lift - JoystickButton autoLift=new JoystickButton(Control.auxStick,1); - autoLift.setHeldAction(new ButtonAction() { - @Override - public void onAction() { - if(Hardware.liftEncoder.getInches().get()<31600) - { - Hardware.motorLiftMainFront.set(1); - Hardware.motorLiftMainBack.set(1); - } - else - { - Hardware.motorLiftMainFront.set(0); - Hardware.motorLiftMainBack.set(0); - } - testMotor2.disable(); - testMotor3.disable(); - } - }); - autoLift.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - Hardware.motorLiftMainFront.set(0); - Hardware.motorLiftMainBack.set(0); - - testMotor2.enable(); - testMotor3.enable(); - } - }); - - - //Half Speed - Button halfSpeedButton=new JoystickButton(Control.auxStick,4); - halfSpeedButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - sensitivity.set(sD*0.5*dir, sT*0.5); - } - }); - - halfSpeedButton.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - sensitivity.set(sD*dir, sT); - } - }); - - //Backwards Button - Button backwardsButton=new JoystickButton(Control.driveStick,12); - backwardsButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - dir=-1; - } - }); - - //Forwards Button - Button forwardsButton=new JoystickButton(Control.driveStick,11); - backwardsButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - dir=1; - } - }); - - //Limiter Off - Button accelLimitOff=new JoystickButton(Control.driveStick,7); - accelLimitOff.setPressAction(new ButtonAction() { - @Override - public void onAction() { - limit.disable(); - } - }); - - //Limiter Off - Button accelLimitOn=new JoystickButton(Control.driveStick,7); - accelLimitOn.setPressAction(new ButtonAction() { - @Override - public void onAction() { - limit.enable(); - } - }); - Motor mBL=new Motor(Hardware.motorDriveBL, false); - Motor mBR=new Motor(Hardware.motorDriveBR, false); - Motor mFL=new Motor(Hardware.motorDriveFL, false); - Motor mFR=new Motor(Hardware.motorDriveFR, false); - - - modules[0] = new DriveModule(new XY(-1, 0), - new XY(0, 1), - Hardware.leftDriveEncoder, - new PID(0.5,0,0), - Module.MotorInputType.PERCENT, - new Motor(Hardware.motorDriveBL), - new Motor(Hardware.motorDriveFL)); - - modules[1] = new DriveModule(new XY(1, 0), - new XY(0, 1), - Hardware.rightDriveEncoder, - new PID(0.5,0,0), - Module.MotorInputType.PERCENT, - new Motor(Hardware.motorDriveBR), - new Motor(Hardware.motorDriveFR)); - - - - DriveTrainBuilder dtBuilder = new DriveTrainBuilder(); - dtBuilder.addDriveModule(modules[0]).addDriveModule(modules[1]); - - /* Build Drive Train */ - - dtBuilder.setInput(Control.driveInput); - dtBuilder.setDriveTrainType(DriveTrainType.TANK); - try { - this.driveTrain = dtBuilder.build(); - } catch (InvalidDriveTrainException e) { - e.printStackTrace(); - } - - /* Drive Train Configurations: Tank, Control */ - - driveTrain.setMapper(new TankMapper()); - driveTrain.setDefaultInput(Control.driveInput); - - /* Drive Train Pipeline: GyroCorrection, Deadzone */ - - - new DashboardInput("auto Selection"); - - ArrayList> steps = new ArrayList<>(); - - correction = new GyroCorrection(Hardware.navx, new PID(1.5, 0, 0.0015), 90, 1); - lock = new GyroLock(correction); - sensitivity=new Sensitivity(sD*dir,sT); - steps.add(new Deadzone()); - steps.add(limit); - steps.add(sensitivity); - steps.add(correction); - driveTrain.setPipeline(new DTPipeline(steps)); - - /* Enabling and Disabling GyroLock */ - - Control.lock.setPressAction(new ButtonAction() { - @Override - public void onAction() { - manualLock = true; - } - }); - - Control.lock.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - manualLock = false; - } - }); - //this.intake = - //new CubeIntake(); - - - - /*super.addAutoMode(new AutoMode("Dynamic auto", new DynamicAutoState())); -new DriveEncoderGyro(12*30,.5,new Degrees(0),false,correction); -*/ - Togglable fieldInput = new FieldCentricDriveInput(Control.driveStick,correction); - new ToggleButton(Control.driveStick,Control.FieldCentricID,fieldInput); - - Button resetButton=new JoystickButton(Control.driveStick,Control.ResetID); - resetButton.setPressAction(new ButtonAction() { - @Override - public void onAction() { - correction.reset(); - } - }); - - - //auto - final double driveSpeed = 0.4; - final int maxEncAccel = 10; - final int maxTicksPerSec = 10; - /*AutoMode autoDrive = new AutoMode("auto Drive", - new DriveEncoderGyro(120, - 0.25, - Angle.ZERO, - true, - correction)); - - - AutoMode encoder = new AutoMode("encoder", - new DriveEncoders(100,.25)); - - AutoMode turn90 = new AutoMode("Turn 90", - new TurnGyro(new Degrees(90),correction,true)); - - AutoMode square = new AutoMode("Square", - new ResetGyro(correction), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(90),correction,false), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(180),correction,false), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(270),correction,false), - new DriveEncoders(4*12,0.25), - new TurnGyro(new Degrees(0),correction,false)); - - AutoMode square2 = new AutoMode("Square 2", - new ResetGyro(correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(0),false,correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(90),false,correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(180),false,correction), - new DriveEncoderGyro(4*12,0.25,new Degrees(270),false,correction)); - -*/ - AutoMode baseLine = new AutoMode("Base Line", - new ResetGyro(correction), - new DriveEncoderGyro(150, .5 , new Degrees(0), false, correction)); - AutoMode centerBaseLineRight = new AutoMode("Center Base Line Right", - new ResetGyro(correction), - new DriveEncoderGyro(60, .5, new Degrees(0), false, correction), - new DriveEncoderGyro(12, .5, new Degrees(90), false, correction), - new DriveEncoderGyro(80, .5, new Degrees(0), false, correction)); - AutoMode centerBaseLineLeft = new AutoMode("Center Base Line Left", - new ResetGyro(correction), - new DriveEncoderGyro(60, .5, new Degrees(0), false, correction), - new DriveEncoderGyro(122, .5, new Degrees(-90), false, correction), - new DriveEncoderGyro(80, .5, new Degrees(0), false, correction)); - - AutoMode switchAuto2=new AutoMode("Switch",new SwitchAuto2(correction)); - -/* - AutoMode twentyFeet=new AutoMode("Twenty Feet", - new ResetGyro(correction), - new DriveEncoderGyro(12*20,.5,new Degrees(0),false,correction)); - addAutoMode(twentyFeet); - - AutoMode tenFeet=new AutoMode("ten Feet", - new ResetGyro(correction), - new DriveEncoderGyro(12*10,.5,new Degrees(0),false,correction)); - addAutoMode(tenFeet); - - AutoMode thirtyFeet=new AutoMode("thirty Feet", - new ResetGyro(correction), - new DriveEncoderGyro(12*30,.5,new Degrees(0),false,correction)); - addAutoMode(thirtyFeet); - AutoMode farAuto = new AutoMode("Rich Mode", - new ResetGyro(correction), - new DriveEncoderGyro(12*4*oldOverNew, .5,new Degrees(0),false,correction), - new DriveEncoderGyro((12*20+4)*oldOverNew,.5,new Degrees(90),false,correction), - new DriveEncoderGyro((12*4)*oldOverNew, .5,new Degrees(180),false,correction), - new Delay(5), - new DriveEncoderGyro((-12*4)*oldOverNew, .5,new Degrees(180),false,correction), - new DriveEncoderGyro((-12*20-4)*oldOverNew,.5,new Degrees(90),false,correction), - new DriveEncoderGyro((-12*4)*oldOverNew, .5,new Degrees(0),false,correction)); - - AutoMode backTen = new AutoMode("Back Ten", - new ResetGyro(correction), - new DriveEncoderGyro(-12*10,.5,new Degrees(0),false,correction)); -*/ addAutoMode(new AutoMode("nothing", new Delay(1))); - addAutoMode(baseLine); - addAutoMode(centerBaseLineLeft); - addAutoMode(centerBaseLineRight); - //addAutoMode(new AutoMode("Switch Auto", new SwitchAuto(correction, intake))); - addAutoMode(switchAuto2); - addAutoMode(new AutoMode("Drive Time",new DriveTime(4,.5))); - sendAutoModes(); - - CameraServer.getInstance().startAutomaticCapture(); - - - // vision stuff - /*UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); - camera.setResolution(IMG_WIDTH, IMG_HEIGHT); - -6gtyj visionThread = new VisionThread(camera, new DrivePipeline(), pipeline -> { - if (!pipeline.filterContoursOutput().isEmpty()) { - org.opencv.core.Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0)); - synchronized (imgLock) { - centerX = r.x + (r.width / 2); - } - } - }); - visionThread.start();*/ - - -/* - AutoMode driveBack20Mode=new AutoMode("DriveBack20",new DriveEncoderGyro(-23,0.5,Angle.ZERO,true,correction)); - - JoystickButton driveBack20=new JoystickButton(Control.driveStick,4); - driveBack20.setPressAction(new ButtonAction() { - @Override - public void onAction() { - driveBack20Mode.start(); - } - }); - driveBack20.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - driveBack20Mode.stop(); - } - }); -*/ - } - - @Override - public void userAutonomousSetup(){ - - } - - - @Override - public void update(){ - /*double centerX; - synchronized (imgLock) { - centerX = this.centerX; - } - double turn = centerX - (IMG_WIDTH / 2); - - SmartDashboard.putNumber("turn", turn); -*/ - SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); - SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); - SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); - SmartDashboard.putNumber("Intake Lift Encoder", Hardware.intakeLiftEncoder.getInches().get()); - SmartDashboard.putNumber("Lift",Hardware.liftEncoder.getInches().get()); - if(SWITCHES) { - SmartDashboard.putBoolean("Intake Open", Hardware.intakeOpenSwitch.get()); - SmartDashboard.putBoolean("Intake Closed", Hardware.intakeClosedSwitch.get()); - } - Debug.msg("Intake Position",Hardware.motorLiftIntake.getSensorCollection().getQuadraturePosition()); - Debug.msg("Intake Velocity",Hardware.motorLiftIntake.getSensorCollection().getQuadraturePosition()); - Hardware.motorLiftIntake.getSensorCollection().getQuadratureVelocity(); - - - gyroLocking(); - - } - - private void gyroLocking(){ - boolean autoLock = ((Math.abs(Control.driveInput.getTurn().toDegrees())<10) && - (Math.abs(Control.driveInput.getDir().getY())>0.5)); - if(autoLock || manualLock){ - lock.enable(); - }else{ - lock.disable(); - } - //lock.update(); - } - - @Override - public void userDisabledPeriodic(){ - SwitchAuto2.disabled(); - } -} diff --git a/src/main/java/frc/team555/robot/test/PowerUpRobotWithLift.java b/src/main/java/frc/team555/robot/test/PowerUpRobotWithLift.java index 6be4cca..0637b16 100644 --- a/src/main/java/frc/team555/robot/test/PowerUpRobotWithLift.java +++ b/src/main/java/frc/team555/robot/test/PowerUpRobotWithLift.java @@ -127,16 +127,16 @@ public void onAction() { /*super.addAutoMode(new AutoMode("Dynamic auto", new DynamicAutoState())); new DriveEncoderGyro(12*30,.5,new Degrees(0),false,correction);*/ - Togglable fieldInput = new FieldCentricDriveInput(Control.driveStick,correction); - new ToggleButton(Control.driveStick,Control.FieldCentricID,fieldInput); + //Togglable fieldInput = new FieldCentricDriveInput(Control.driveStick,correction); + //new ToggleButton(Control.driveStick,Control.FieldCentricID,fieldInput); - Button resetButton=new JoystickButton(Control.driveStick,Control.ResetID); - resetButton.setPressAction(new ButtonAction() { + //Button resetButton=new JoystickButton(Control.driveStick,Control.ResetID); + /*resetButton.setPressAction(new ButtonAction() { @Override public void onAction() { correction.reset(); } - }); + });*/ //Lift diff --git a/src/main/java/frc/team555/robot/test/TestRobot.java b/src/main/java/frc/team555/robot/test/TestRobot.java index a72029a..5303f88 100644 --- a/src/main/java/frc/team555/robot/test/TestRobot.java +++ b/src/main/java/frc/team555/robot/test/TestRobot.java @@ -4,6 +4,7 @@ import frc.team555.robot.components.IntakeLift; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; +import frc.team555.robot.utils.JoystickXAxis; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; import org.montclairrobotics.sprocket.control.JoystickYAxis; @@ -49,6 +50,7 @@ public Double get() { Input input=new JoystickYAxis(Control.auxStick); ControlledMotor testMotor1 = new ControlledMotor(Hardware.motorLiftMainFront, input); ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorLiftMainBack, input); + testMotor2.setInverted(true); super.addAutoMode(new AutoMode("name")); super.sendAutoModes(); diff --git a/src/main/java/frc/team555/robot/test/TestRobot2.java b/src/main/java/frc/team555/robot/test/TestRobot2.java index 478810b..edda41a 100644 --- a/src/main/java/frc/team555/robot/test/TestRobot2.java +++ b/src/main/java/frc/team555/robot/test/TestRobot2.java @@ -46,9 +46,21 @@ public Double get() { } };*/ - Input input=new JoystickYAxis(Control.auxStick); - ControlledMotor testMotor1 = new ControlledMotor(Hardware.motorLiftMainBack, input); - ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorLiftMainFront, input); + Input rInput = new Input() { + @Override + public Double get() { + return Control.auxStick.getY(); + } + }; + + Input lInput = new Input() { + @Override + public Double get() { + return -Control.auxStick.getY(); + } + }; + ControlledMotor testMotor1 = new ControlledMotor(Hardware.motorIntakeR, rInput); + ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorIntakeL, lInput); super.addAutoMode(new AutoMode("name")); super.sendAutoModes(); diff --git a/src/main/java/frc/team555/robot/test/TestRobot3.java b/src/main/java/frc/team555/robot/test/TestRobot3.java deleted file mode 100644 index 735fb89..0000000 --- a/src/main/java/frc/team555/robot/test/TestRobot3.java +++ /dev/null @@ -1,64 +0,0 @@ -package frc.team555.robot.test; - -import frc.team555.robot.auto.ConditionalState; -import frc.team555.robot.components.CubeIntake; -import frc.team555.robot.components.IntakeLift; -import frc.team555.robot.core.Control; -import frc.team555.robot.core.Hardware; -import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.control.JoystickYAxis; -import org.montclairrobotics.sprocket.drive.ControlledMotor; -import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.Input; - -public class TestRobot3 extends SprocketRobot{ - IntakeLift intakeLift; - CubeIntake intake; - - public void robotInit() { - Hardware.init(); - Control.init(); - - - - /*Input yAxis = new JoystickYAxis(0); - Input xAxisA = new JoystickXAxis(0); - Input xAxis= new Input(){ - - @Override - public Double get() { - if(Math.abs(xAxisA.get())<0.25) - { - return 0.0; - } - return xAxisA.get(); - } - }; - Input leftInput = new Input() { - @Override - public Double get() { - return yAxis.get() - xAxis.get(); - } - }; - Input rightInput = new Input() { - @Override - public Double get() { - return yAxis.get() + xAxis.get(); - } - };*/ - - Input input=new JoystickYAxis(Control.auxStick); - ControlledMotor testMotor1 = new ControlledMotor(Hardware.motorIntakeClamp, input); - - - input=new JoystickYAxis(Control.driveStick); - ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorLiftMainBack, input); - ControlledMotor testMotor3 = new ControlledMotor(Hardware.motorLiftMainFront, input); - - //Debug.msg("Lift Ticks",Hardware.liftEncoder.getInches().get()); - - super.addAutoMode(new AutoMode("name")); - super.sendAutoModes(); - } -} diff --git a/src/main/java/frc/team555/robot/utils/BangBang.java b/src/main/java/frc/team555/robot/utils/BangBang.java new file mode 100644 index 0000000..8b0b683 --- /dev/null +++ b/src/main/java/frc/team555/robot/utils/BangBang.java @@ -0,0 +1,24 @@ +package frc.team555.robot.utils; + + +import org.montclairrobotics.sprocket.utils.PID; + +public class BangBang extends PID { + public double tolerance=1,power=1; + public BangBang(double tolerance,double power) + { + this.tolerance=tolerance; + this.power=power; + } + + public double get() + { + double error=super.getTarget()-super.getCurInput(); + if(error>tolerance) + return power; + if(error0 && upperBound.get()) + { + power=0; + } + if(power<0 && lowerBound.get()) + { + power=0; + } + super.set(power); + } +} diff --git a/src/main/java/frc/team555/robot/utils/POVButton.java b/src/main/java/frc/team555/robot/utils/POVButton.java new file mode 100644 index 0000000..2d5ae70 --- /dev/null +++ b/src/main/java/frc/team555/robot/utils/POVButton.java @@ -0,0 +1,21 @@ +package frc.team555.robot.utils; + + +import edu.wpi.first.wpilibj.Joystick; +import org.montclairrobotics.sprocket.control.Button; + +public class POVButton extends Button { + + Joystick stick; + int pov; + public POVButton(Joystick stick,int pov) + { + this.stick=stick; + this.pov=pov; + } + + @Override + public Boolean get() { + return stick.getPOV()==pov; + } +} diff --git a/src/main/java/frc/team555/robot/utils/TargetMotor.java b/src/main/java/frc/team555/robot/utils/TargetMotor.java index bfaf839..8be8d0e 100644 --- a/src/main/java/frc/team555/robot/utils/TargetMotor.java +++ b/src/main/java/frc/team555/robot/utils/TargetMotor.java @@ -8,6 +8,8 @@ import org.montclairrobotics.sprocket.motors.Module; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; +import org.montclairrobotics.sprocket.utils.Debug; +import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; @@ -18,7 +20,8 @@ */ public class TargetMotor extends Module implements Updatable{ public enum Mode {POSITION,POWER}; - + private double power=0; + public Input upperBound,lowerBound; /** * The encoder attached to the motor */ @@ -60,9 +63,10 @@ public void setTarget(double target){ pid.setTarget(target); } - public void setPower(double power){ + public void setPower(double pow){ mode = Mode.POWER; - super.set(power); + this.power=pow; + safeSet(pow); } /** @@ -73,6 +77,11 @@ public void reset(){ pid.setTarget(0); encoder.reset(); } + + public void stop() + { + setPower(0); + } /** * Update the PID and set the according motor power @@ -80,8 +89,22 @@ public void reset(){ @Override public void update() { if(mode==Mode.POSITION) { - super.set(pid.get()); + safeSet(pid.get()); } + else + { + //safeSet(power); + } + } + + public void safeSet(double pow) + { + /*if(pow>0 &&(upperBound!=null && upperBound.get()) || pow<0 && (lowerBound!=null && lowerBound.get())) + { + pow=0; + }*/ + Debug.msg("Setting a power",pow); + super.set(pow); } public double getTarget(){