From 21fc68b27ec9ec1f1114e6bd750b1554b76e9bce Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Thu, 22 Mar 2018 15:36:37 -0400 Subject: [PATCH 01/38] Stuff --- src/main/java/frc/team555/robot/Robot.java | 5 +- .../frc/team555/robot/auto/SwitchAuto.java | 5 +- .../frc/team555/robot/auto/SwitchAuto2.java | 10 +- .../team555/robot/components/CubeIntake.java | 89 +--- .../team555/robot/components/IntakeLift.java | 4 +- .../robot/components/SimpleIntake.java | 129 ----- .../java/frc/team555/robot/core/Control.java | 57 +- .../java/frc/team555/robot/core/Hardware.java | 10 +- .../frc/team555/robot/core/PowerUpRobot.java | 4 +- .../frc/team555/robot/core/PowerUpRobot2.java | 502 ------------------ .../frc/team555/robot/test/PIDLoopTest.java | 500 ----------------- .../frc/team555/robot/test/TestRobot3.java | 64 --- .../frc/team555/robot/utils/BangBang.java | 24 + 13 files changed, 100 insertions(+), 1303 deletions(-) delete mode 100644 src/main/java/frc/team555/robot/components/SimpleIntake.java delete mode 100644 src/main/java/frc/team555/robot/core/PowerUpRobot2.java delete mode 100644 src/main/java/frc/team555/robot/test/PIDLoopTest.java delete mode 100644 src/main/java/frc/team555/robot/test/TestRobot3.java create mode 100644 src/main/java/frc/team555/robot/utils/BangBang.java diff --git a/src/main/java/frc/team555/robot/Robot.java b/src/main/java/frc/team555/robot/Robot.java index 76a31c2..c8785c1 100644 --- a/src/main/java/frc/team555/robot/Robot.java +++ b/src/main/java/frc/team555/robot/Robot.java @@ -1,12 +1,9 @@ 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.TestRobot; import frc.team555.robot.test.TestRobot2; -import frc.team555.robot.test.TestRobot3; -public class Robot extends PowerUpRobot2 { +public class Robot extends PowerUpRobot { } diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 3cf0b11..0f1ce8f 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -3,7 +3,6 @@ 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.SimpleIntake; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; import org.montclairrobotics.sprocket.auto.states.ResetGyro; @@ -35,9 +34,9 @@ public static void disabled(){ SmartDashboard.putData(startSidesChooser); } - public SwitchAuto(GyroCorrection correction, SimpleIntake intake){ + public SwitchAuto(GyroCorrection correction, CubeIntake intake){ super(new ResetGyro(correction), new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)); - //new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide)); + // new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide)); } } diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java index 86aabe6..3cb4ac0 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java @@ -45,7 +45,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)), @@ -58,8 +58,8 @@ 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 DriveTime(3, .3)); + //new MoveIntake(.8,0.5)); } } @@ -84,7 +84,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 +102,6 @@ public void userStop() { Hardware.motorIntakeClamp.set(0); } - } + }*/ } diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index bee3624..80e7d75 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -2,6 +2,7 @@ import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; +import frc.team555.robot.utils.BangBang; import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; import org.montclairrobotics.sprocket.geometry.Vector; @@ -10,7 +11,6 @@ import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.Togglable; @@ -18,11 +18,13 @@ public class CubeIntake implements Updatable, Togglable{ + private double rotatePower=0.5; + + public final Motor left; public final Motor right; - public final Motor clamp; public final TargetMotor roationalMotor; - public final double tolerance = .01; // Todo: Maybe needs to be tuned + public final double tolerance = 1; // Todo: Maybe needs to be tuned public final int upPos = 0; public final int downPos = 2; @@ -31,17 +33,12 @@ public class CubeIntake implements Updatable, Togglable{ public final 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 - public CubeIntake() { this.left = new Motor(Hardware.motorIntakeL); this.right = new Motor(Hardware.motorIntakeR); - this.clamp = new Motor(Hardware.motorIntakeClamp); - this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new PID(), new Motor(Hardware.motorRotational)); // Todo: needs to be implemented - + //this.clamp = new Motor(Hardware.motorIntakeClamp); + this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), new Motor(Hardware.motorIntakeRotate)); // Todo: needs to be implemented + this.power = new Input() { @Override public Vector get() { @@ -53,20 +50,20 @@ public Vector get() { }; - Control.intakeRotationDown.setPressAction(new ButtonAction() { + Control.intakeRotateDown.setPressAction(new ButtonAction() { @Override public void onAction() { roationalMotor.set(downPos); } }); - Control.intakeRotationUp.setPressAction(new ButtonAction() { + Control.intakeRotateUp.setPressAction(new ButtonAction() { @Override public void onAction() { roationalMotor.set(upPos); } }); - Control.intakeRotationMiddle.setPressAction(new ButtonAction() { + Control.intakeRotateMiddle.setPressAction(new ButtonAction() { @Override public void onAction() { roationalMotor.set(middlePos); @@ -74,7 +71,19 @@ public void onAction() { }); + Control.intakeRotateUpManual.setPressAction(new ButtonAction() { + @Override + public void onAction() { + roationalMotor.setPower(rotatePower); + } + }); + Control.intakeRotateUpManual.setPressAction(new ButtonAction() { + @Override + public void onAction() { + roationalMotor.setPower(-rotatePower); + } + }); Updater.add(this, Priority.CALC); } @@ -83,14 +92,9 @@ public void onAction() { @Override public void update() { Vector p = power.get(); - if(p.getMagnitude() < tolerance){ - openClamp(); - }else{ - closeClamp(); - } + left.set(p.getX()); right.set(p.getY()); - updateClamp(); } @Override @@ -105,49 +109,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()-clampStartSystem.currentTimeMillis()) - { - clampStart=System.currentTimeMillis(); - } - } - - private void updateClamp() - { - if(System.currentTimeMillis()-clampStart 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/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(error Date: Thu, 22 Mar 2018 15:57:10 -0400 Subject: [PATCH 02/38] Finished up rewriting, and configuring --- src/main/java/frc/team555/robot/core/Hardware.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 36dba84..b5ff05f 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -59,6 +59,10 @@ private static class DeviceID { public static final int motorIntakeClamp = 6; public static final int motorIntakeRotate = 10; + + public static final int motorIntakeL = 11; + public static final int motorIntakeR = 12; + //left intake 5 //pincer 6 //lifter 9 @@ -131,6 +135,9 @@ public static void init(){ motorLiftMainBack = new WPI_TalonSRX(DeviceID.motorMainLiftBack); motorIntakeRotate = new WPI_TalonSRX(DeviceID.motorIntakeRotate); + motorIntakeR = new WPI_TalonSRX(DeviceID.motorIntakeR); + motorIntakeL = new WPI_TalonSRX(DeviceID.motorIntakeL); + motorLiftMainBack.setInverted(true); //motorIntakeClamp = new WPI_TalonSRX(DeviceID.motorIntakeClamp); @@ -163,11 +170,11 @@ public static void init(){ //15.5 to 49.5 liftEncoder = new SEncoder(new Encoder(4,5), 31600/(82 - 54)); // todo: REALLY NEED TO BE SET intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1333600/(49.5 - 15.5)); - intakeRotationEncoder = new SEncoder(new Encoder(0, 0), 1); // todo, needs to be set + intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // todo, needs to be set navx = new NavXInput(DeviceID.navxPort); - liftLimitSwitch = new DigitalInput(10); + liftLimitSwitch = new DigitalInput(6); } } From df98988cd1aa6fc6e753936a2e5342c25374f50c Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Thu, 22 Mar 2018 16:02:21 -0400 Subject: [PATCH 03/38] THIS is the correct config --- src/main/java/frc/team555/robot/core/Hardware.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index b5ff05f..67b0491 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -56,12 +56,12 @@ private static class DeviceID { public static final int motorMainLiftFront = 4; public static final int motorMainLiftBack = 2; - public static final int motorIntakeClamp = 6; + //public static final int motorIntakeClamp = 6; - public static final int motorIntakeRotate = 10; + public static final int motorIntakeRotate = 6; - public static final int motorIntakeL = 11; - public static final int motorIntakeR = 12; + public static final int motorIntakeL = 10; + public static final int motorIntakeR = 5; //left intake 5 //pincer 6 From 65372b640bc5f0080f54b7263f4d5a1a4149141b Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Thu, 22 Mar 2018 18:53:13 -0400 Subject: [PATCH 04/38] Its all broken in every way --- .../java/frc/team555/robot/auto/DropCube.java | 20 +++++++-- .../java/frc/team555/robot/auto/SideTurn.java | 13 +++++- .../frc/team555/robot/auto/SwitchAuto.java | 16 ++++++-- .../team555/robot/components/CubeIntake.java | 41 +++++++++++++++---- .../team555/robot/components/IntakeLift.java | 25 +++++------ .../team555/robot/components/MainLift.java | 25 ++++++----- .../java/frc/team555/robot/core/Control.java | 8 ++-- .../frc/team555/robot/core/PowerUpRobot.java | 11 ++++- .../frc/team555/robot/test/TestRobot2.java | 4 +- 9 files changed, 117 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/DropCube.java b/src/main/java/frc/team555/robot/auto/DropCube.java index 63a019a..cc129ed 100644 --- a/src/main/java/frc/team555/robot/auto/DropCube.java +++ b/src/main/java/frc/team555/robot/auto/DropCube.java @@ -3,17 +3,29 @@ 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.DriveTime; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; +import org.montclairrobotics.sprocket.states.State; import org.montclairrobotics.sprocket.states.StateMachine; public class DropCube extends StateMachine{ - public DropCube(CubeIntake intake, MainLift lift, GyroCorrection correction, Side side){ + public DropCube(CubeIntake intake, GyroCorrection correction, Side side){ super( new SideTurn(correction, false, side), - new MainLiftMove(12,.5,true), - new DriveTime(3,.3), - new CubeOuttake(intake, 5) + // new MainLiftMove(12,.5,true), + new DriveTime(3, .3), + new AutoState() { + @Override + public void stateUpdate() { + intake.auto(); + } + + @Override + public boolean isDone() { + return super.timeInState()>2; + } + } ); } } diff --git a/src/main/java/frc/team555/robot/auto/SideTurn.java b/src/main/java/frc/team555/robot/auto/SideTurn.java index e8b96a9..2489078 100644 --- a/src/main/java/frc/team555/robot/auto/SideTurn.java +++ b/src/main/java/frc/team555/robot/auto/SideTurn.java @@ -1,20 +1,31 @@ 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; public class SideTurn extends TurnGyro { public Side side; + private boolean relative; + private GyroCorrection gyro; public SideTurn( GyroCorrection gyro, boolean relative, Side side) { super(Angle.ZERO, gyro, relative); this.side = side; } - + 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){ diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 0f1ce8f..a0abf9d 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -3,8 +3,11 @@ 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.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; @@ -31,12 +34,17 @@ public Boolean get() { } public static void disabled(){ - SmartDashboard.putData(startSidesChooser); + SmartDashboard.putData("Start Position", startSidesChooser); + } + + public static void loop(){ + SmartDashboard.putBoolean("Correct Side", startSide.get()); } - public SwitchAuto(GyroCorrection correction, CubeIntake intake){ + public SwitchAuto(GyroCorrection correction, CubeIntake intake, IntakeLift lift){ super(new ResetGyro(correction), - new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)); - // new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide)); + new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction), + new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide) + ); } } diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 80e7d75..cb51343 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -18,8 +18,9 @@ public class CubeIntake implements Updatable, Togglable{ - private double rotatePower=0.5; + private double rotatePower=1; + private boolean auto; public final Motor left; public final Motor right; @@ -36,15 +37,17 @@ public class CubeIntake implements Updatable, Togglable{ public CubeIntake() { this.left = new Motor(Hardware.motorIntakeL); this.right = new Motor(Hardware.motorIntakeR); + right.setInverted(true); //this.clamp = new Motor(Hardware.motorIntakeClamp); - this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), new Motor(Hardware.motorIntakeRotate)); // Todo: needs to be implemented + Motor rotateMotor=new Motor(Hardware.motorIntakeRotate); + 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() + -Control.auxStick.getX(), // + Control.auxStick.getX(), + -Control.auxStick.getY()// - Control.auxStick.getX() ); } }; @@ -77,14 +80,27 @@ public void onAction() { roationalMotor.setPower(rotatePower); } }); + Control.intakeRotateUpManual.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + roationalMotor.setPower(0); + } + }); - Control.intakeRotateUpManual.setPressAction(new ButtonAction() { + Control.intakeRotateDownManual.setPressAction(new ButtonAction() { @Override public void onAction() { roationalMotor.setPower(-rotatePower); } }); - + Control.intakeRotateDownManual.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + roationalMotor.setPower(0); + } + }); + + roationalMotor.setPower(0); Updater.add(this, Priority.CALC); } @@ -93,10 +109,19 @@ public void onAction() { public void update() { Vector p = power.get(); - left.set(p.getX()); - right.set(p.getY()); + if(!auto) { + left.set(p.getY() - p.getX()); + right.set(p.getY() + p.getX()); + } + auto=false; } + + public void auto() + { + auto=true; + enable(); + } @Override public void enable() { left.set(-.5); diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 5e9881e..90d3528 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -2,6 +2,7 @@ import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; +import frc.team555.robot.utils.BangBang; import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; import org.montclairrobotics.sprocket.motors.Motor; @@ -18,7 +19,7 @@ public class IntakeLift { public final int liftDownPosition = 0;*/ public double MANUAL_POWER=.5; - public final double[] positions = {0D, 333D, 667D, 1000D}; // Todo: test values + public final double[] positions = {0D, 49-15}; // Todo: test values private int pos; private TargetMotor motors; SEncoder encoder; @@ -26,7 +27,7 @@ public class IntakeLift { * Constructor for IntakeLift Class with default position of 0 */ public IntakeLift() { - motors = new TargetMotor(Hardware.liftEncoder, new PID(0, 0, 0),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng + motors = new TargetMotor(Hardware.liftEncoder, new BangBang(1,1),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng encoder=Hardware.liftEncoder; //================= @@ -68,18 +69,18 @@ public void onAction() { */ //Start the motors in manual mode //IntakeLift up (manual backup control) - Control.intakeLiftManualUp.setHeldAction(new ButtonAction() { + Control.intakeLiftManualUp.setPressAction(new ButtonAction() { @Override public void onAction() { - setPower(MANUAL_POWER); + motors.setPower(MANUAL_POWER); } }); //IntakeLift down (manual backup control) - Control.intakeLiftManualDown.setHeldAction(new ButtonAction() { + Control.intakeLiftManualDown.setPressAction(new ButtonAction() { @Override public void onAction() { - setPower(-MANUAL_POWER); + motors.setPower(-MANUAL_POWER); } }); @@ -88,24 +89,24 @@ public void onAction() { ButtonAction stop=new ButtonAction() { @Override public void onAction() { - setPower(0); + motors.setPower(0); } }; - Control.intakeLiftManualUp.setOffAction(stop); - Control.intakeLiftManualDown.setOffAction(stop); - + 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); } diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 442d397..882c626 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -2,11 +2,14 @@ import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; +import frc.team555.robot.utils.BangBang; import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.utils.PID; +import java.nio.channels.Pipe; + public class MainLift extends TargetMotor { private final double speed = 1.0; @@ -15,7 +18,7 @@ public class MainLift extends TargetMotor { private int downPosition; public MainLift(){ - super(Hardware.liftEncoder, new PID(0, 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; @@ -23,31 +26,34 @@ public MainLift(){ Control.mainLiftManualUp.setPressAction(new ButtonAction() { @Override public void onAction() { - MainLift.super.set(speed); + set(speed); } }); Control.mainLiftManualUp.setReleaseAction(new ButtonAction() { @Override public void onAction() { - MainLift.super.set(0); + set(0); } }); // Manual Down - Control.mainLiftManualDown.setPressAction(new ButtonAction() { + Control.mainLiftManualDown.setHeldAction(new ButtonAction() { @Override public void onAction() { - MainLift.super.set(-speed); + if(!Hardware.liftLimitSwitch.get()) { + set(-speed); + }else{ + set(0); + encoder.reset(); + } } }); Control.mainLiftManualDown.setReleaseAction(new ButtonAction() { @Override public void onAction() { - if(encoder != null) { - MainLift.super.setTarget(upPosition); - } + set(0); } }); @@ -60,8 +66,7 @@ public void onAction() { } } }); - - + setPower(0); // Lift Bottom diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index fcdbc95..a2dae89 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -68,10 +68,10 @@ public static void init() { //intakeLiftDecrement = new JoystickButton(auxStick,12); mainLiftTop = new JoystickButton(auxStick,9); mainLiftBottom = new JoystickButton(auxStick,8); - mainLiftManualUp = new JoystickButton(auxStick,10); - mainLiftManualDown = new JoystickButton(auxStick,11); - intakeLiftManualUp = new JoystickButton(auxStick,6); - intakeLiftManualDown = new JoystickButton(auxStick,7); + mainLiftManualUp = new JoystickButton(auxStick,11); + mainLiftManualDown = new JoystickButton(auxStick,10); + intakeLiftManualUp = new JoystickButton(auxStick,7); + intakeLiftManualDown = new JoystickButton(auxStick,6); //intakeLiftTop = new JoystickButton(auxStick, 12); //intakeLiftBottom = new JoystickButton(auxStick, 13); //intakeSubroutine = new JoystickButton(auxStick, 14); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index ac8b073..e74f06d 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.components.IntakeLift; +import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; @@ -30,6 +32,8 @@ public class PowerUpRobot extends SprocketRobot { GyroLock lock; boolean manualLock; CubeIntake intake; + MainLift mainLift; + IntakeLift intakeLift; //vision stuff private static final int IMG_WIDTH = 320; @@ -55,6 +59,8 @@ public void robotInit(){ SwitchAuto.init(); DriveModule[] modules = new DriveModule[2]; intake = new CubeIntake(); + mainLift=new MainLift(); + intakeLift=new IntakeLift(); modules[0] = new DriveModule(new XY(-1, 0), new XY(0, 1), @@ -222,6 +228,7 @@ public void onAction() { addAutoMode(baseLine); addAutoMode(centerBaseLineLeft); addAutoMode(centerBaseLineRight); + addAutoMode(new AutoMode("switch", new SwitchAuto(correction, intake, intakeLift))); //addAutoMode(new AutoMode("Switch Auto", new SwitchAuto(correction, intake))); sendAutoModes(); @@ -265,6 +272,7 @@ public void onAction() { } }); visionThread.start();*/ + intakeLift.setPower(0); } @Override @@ -286,8 +294,9 @@ public void update(){ SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); + SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); gyroLocking(); - + SwitchAuto.loop(); } private void gyroLocking(){ diff --git a/src/main/java/frc/team555/robot/test/TestRobot2.java b/src/main/java/frc/team555/robot/test/TestRobot2.java index 478810b..1f92227 100644 --- a/src/main/java/frc/team555/robot/test/TestRobot2.java +++ b/src/main/java/frc/team555/robot/test/TestRobot2.java @@ -47,8 +47,8 @@ public Double get() { };*/ Input input=new JoystickYAxis(Control.auxStick); - ControlledMotor testMotor1 = new ControlledMotor(Hardware.motorLiftMainBack, input); - ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorLiftMainFront, input); + ControlledMotor testMotor1 = new ControlledMotor(Hardware.motorIntakeR, input); + ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorIntakeL, input); super.addAutoMode(new AutoMode("name")); super.sendAutoModes(); From 1df05d32f861d220c9ee0dd4f435338270bf2922 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 23 Mar 2018 16:50:21 -0400 Subject: [PATCH 05/38] Change the move lift auto state to make it simpler and hopefully work --- .../java/frc/team555/robot/auto/SetIntakeLift.java | 10 +++++----- src/main/java/frc/team555/robot/auto/SideTurn.java | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/SetIntakeLift.java b/src/main/java/frc/team555/robot/auto/SetIntakeLift.java index d784627..cff149c 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeLift.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeLift.java @@ -1,6 +1,7 @@ package frc.team555.robot.auto; import frc.team555.robot.components.IntakeLift; +import frc.team555.robot.core.Hardware; import org.montclairrobotics.sprocket.states.State; public class SetIntakeLift implements State { @@ -21,22 +22,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/SideTurn.java b/src/main/java/frc/team555/robot/auto/SideTurn.java index 2489078..5cd4b14 100644 --- a/src/main/java/frc/team555/robot/auto/SideTurn.java +++ b/src/main/java/frc/team555/robot/auto/SideTurn.java @@ -17,6 +17,7 @@ public class SideTurn extends TurnGyro { public SideTurn( GyroCorrection gyro, boolean relative, Side side) { super(Angle.ZERO, gyro, relative); this.side = side; + this.gyro=gyro; } public void userStart() { super.userStart(); From 2cc95339994ab036875b01909bc1b1a033c88e66 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 23 Mar 2018 17:04:02 -0400 Subject: [PATCH 06/38] Control and hardware changes --- .../java/frc/team555/robot/core/Control.java | 54 ++++++++++--------- .../java/frc/team555/robot/core/Hardware.java | 4 +- .../frc/team555/robot/core/PowerUpRobot.java | 2 +- 3 files changed, 31 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index a2dae89..91c5c2f 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -23,17 +23,18 @@ public class Control { intakeLiftManualDown, //intakeLiftTop, //intakeLiftBottom, + mainLiftAutoUp, mainLiftManualUp, - mainLiftManualDown, - mainLiftTop, - mainLiftBottom, + mainLiftManualDown; + // mainLiftTop, + // mainLiftBottom, //openButton, //closeButton, - intakeRotateUp, - intakeRotateDown, - intakeRotateMiddle, - intakeRotateUpManual, - intakeRotateDownManual; + //intakeRotateUp, + //intakeRotateDown, + //intakeRotateMiddle, + //intakeRotateUpManual, + //intakeRotateDownManual; //intakeSubroutine; @@ -64,28 +65,29 @@ public static void init() { //IntakeLift Buttons - //intakeLiftIncrement = new JoystickButton(auxStick,12); - //intakeLiftDecrement = new JoystickButton(auxStick,12); - mainLiftTop = new JoystickButton(auxStick,9); - mainLiftBottom = new JoystickButton(auxStick,8); - mainLiftManualUp = new JoystickButton(auxStick,11); - mainLiftManualDown = new JoystickButton(auxStick,10); - intakeLiftManualUp = new JoystickButton(auxStick,7); - intakeLiftManualDown = new JoystickButton(auxStick,6); - //intakeLiftTop = new JoystickButton(auxStick, 12); - //intakeLiftBottom = new JoystickButton(auxStick, 13); - //intakeSubroutine = new JoystickButton(auxStick, 14); + // intakeLiftIncrement = new JoystickButton(auxStick,12); + // intakeLiftDecrement = new JoystickButton(auxStick,12); + // mainLiftTop = new JoystickButton(auxStick,9); + // mainLiftBottom = new JoystickButton(auxStick,8); + mainLiftManualUp = new JoystickButton(auxStick,3); + mainLiftManualDown = new JoystickButton(auxStick,2); + intakeLiftManualUp = new JoystickButton(auxStick,5); + intakeLiftManualDown = new JoystickButton(auxStick,4); + mainLiftAutoUp = new JoystickButton(auxStick, 1); + // intakeLiftTop = new JoystickButton(auxStick, 12); + // intakeLiftBottom = new JoystickButton(auxStick, 13); + // intakeSubroutine = new JoystickButton(auxStick, 14); - //openButton=new JoystickButton(Control.auxStick,8); - //closeButton=new JoystickButton(Control.auxStick,9); + // openButton=new JoystickButton(Control.auxStick,8); + // closeButton=new JoystickButton(Control.auxStick,9); //Intake Buttons - intakeRotateUp = new JoystickButton(auxStick, 12); - intakeRotateDown = new JoystickButton(auxStick, 4); - intakeRotateMiddle=new JoystickButton(auxStick,5); - intakeRotateUpManual = new JoystickButton(auxStick, 3); - intakeRotateDownManual = new JoystickButton(auxStick, 2); + // intakeRotateUp = new JoystickButton(auxStick, 12); + // intakeRotateDown = new JoystickButton(auxStick, 4); + // intakeRotateMiddle=new JoystickButton(auxStick,5); + // intakeRotateUpManual = new JoystickButton(auxStick, 3); + // intakeRotateDownManual = new JoystickButton(auxStick, 2); // Auxiliary Buttons diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 67b0491..4dc1eb3 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -168,8 +168,8 @@ public static void init(){ //Intake Lift Travel //15.5 to 49.5 - liftEncoder = new SEncoder(new Encoder(4,5), 31600/(82 - 54)); // todo: REALLY NEED TO BE SET - intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1333600/(49.5 - 15.5)); + liftEncoder = new SEncoder(new Encoder(4,5), 1/*31600/(82 - 54)*/); // todo: REALLY NEED TO BE SET + intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1 /*1333600/(49.5 - 15.5)*/); intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // todo, needs to be set diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index e74f06d..b009c7e 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -135,7 +135,7 @@ public void onAction() { */ 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 From 9e42f0548f93781c4a339d3b21466c3ddd0a830d Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 23 Mar 2018 20:42:41 -0400 Subject: [PATCH 07/38] Some changes that will either fix a lot or break even more. --- .../frc/team555/robot/auto/CubeOuttake.java | 2 +- .../frc/team555/robot/auto/MainLiftMove.java | 1 + .../java/frc/team555/robot/auto/MoveLift.java | 48 +++++++++++++++++++ .../frc/team555/robot/auto/SetIntakeLift.java | 1 + .../java/frc/team555/robot/auto/SideTurn.java | 6 +++ .../frc/team555/robot/auto/SwitchAuto.java | 4 +- .../team555/robot/components/CubeIntake.java | 23 +++++---- .../team555/robot/components/IntakeLift.java | 22 +++++++-- .../frc/team555/robot/components/Lift.java | 8 ++++ .../team555/robot/components/MainLift.java | 40 +++++++++++++++- .../frc/team555/robot/core/PowerUpRobot.java | 17 ++++++- 11 files changed, 154 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/MoveLift.java create mode 100644 src/main/java/frc/team555/robot/components/Lift.java diff --git a/src/main/java/frc/team555/robot/auto/CubeOuttake.java b/src/main/java/frc/team555/robot/auto/CubeOuttake.java index ceed9ca..d580bb6 100644 --- a/src/main/java/frc/team555/robot/auto/CubeOuttake.java +++ b/src/main/java/frc/team555/robot/auto/CubeOuttake.java @@ -5,7 +5,7 @@ import org.montclairrobotics.sprocket.auto.states.Disable; import org.montclairrobotics.sprocket.auto.states.Enable; import org.montclairrobotics.sprocket.states.StateMachine; - +@Deprecated public class CubeOuttake extends StateMachine { public CubeOuttake(CubeIntake intake, double time){ 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/SetIntakeLift.java b/src/main/java/frc/team555/robot/auto/SetIntakeLift.java index cff149c..8be9d15 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeLift.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeLift.java @@ -4,6 +4,7 @@ import frc.team555.robot.core.Hardware; import org.montclairrobotics.sprocket.states.State; +@Deprecated public class SetIntakeLift implements State { static final double power = .5; diff --git a/src/main/java/frc/team555/robot/auto/SideTurn.java b/src/main/java/frc/team555/robot/auto/SideTurn.java index 5cd4b14..434a3ef 100644 --- a/src/main/java/frc/team555/robot/auto/SideTurn.java +++ b/src/main/java/frc/team555/robot/auto/SideTurn.java @@ -36,5 +36,11 @@ public Angle getTgt(){ } } + @Override + public boolean isDone() + { + return super.isDone()||super.timeInState()>5; + } + } diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index a0abf9d..84fc4c3 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -4,6 +4,7 @@ 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.utils.Side; import org.montclairrobotics.sprocket.auto.states.Delay; import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; @@ -41,8 +42,9 @@ public static void loop(){ SmartDashboard.putBoolean("Correct Side", startSide.get()); } - public SwitchAuto(GyroCorrection correction, CubeIntake intake, IntakeLift lift){ + public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake, IntakeLift lift){ super(new ResetGyro(correction), + new MoveLift(mainLift,MainLift.TOP*0.6,1,true), new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction), new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide) ); diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index cb51343..918fb70 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -45,15 +45,22 @@ public CubeIntake() { this.power = new Input() { @Override public Vector get() { - return new XY( - -Control.auxStick.getX(), // + Control.auxStick.getX(), - -Control.auxStick.getY()// - Control.auxStick.getX() - ); + double x=-Control.auxStick.getX(); + if(Math.abs(x)<0.2) + { + x=0; + } + double y=-Control.auxStick.getY(); + if(Math.abs(y)<0.2) + { + y=0; + } + return new XY(x,y); } }; - Control.intakeRotateDown.setPressAction(new ButtonAction() { + /*Control.intakeRotateDown.setPressAction(new ButtonAction() { @Override public void onAction() { roationalMotor.set(downPos); @@ -99,7 +106,7 @@ public void onAction() { roationalMotor.setPower(0); } }); - +*/ roationalMotor.setPower(0); Updater.add(this, Priority.CALC); } @@ -124,8 +131,8 @@ public void auto() } @Override public void enable() { - left.set(-.5); - right.set(-.5); + left.set(1); + right.set(1); } @Override diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 90d3528..9b744be 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -14,21 +14,24 @@ /** * The IntakeLift class manages the motors, positions, and PID for the intakeLift on the PowerUp robot. */ -public class IntakeLift { +public class IntakeLift implements Lift{ /* public final int liftUpPosition = 1000; public final int liftDownPosition = 0;*/ + private boolean auto=false; + + public double MANUAL_POWER=.5; public final double[] positions = {0D, 49-15}; // Todo: test values private int pos; private TargetMotor motors; - SEncoder encoder; + public SEncoder encoder; /** * Constructor for IntakeLift Class with default position of 0 */ public IntakeLift() { - motors = new TargetMotor(Hardware.liftEncoder, new BangBang(1,1),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng - encoder=Hardware.liftEncoder; + motors = new TargetMotor(Hardware.intakeLiftEncoder, new BangBang(1,1),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng + encoder=Hardware.intakeLiftEncoder; //================= //Buttons below!!!! @@ -110,6 +113,17 @@ public void setPositionRaw(double 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 882c626..0ad021c 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -6,13 +6,18 @@ import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; import org.montclairrobotics.sprocket.motors.Motor; +import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.utils.PID; import java.nio.channels.Pipe; -public class MainLift extends TargetMotor { +public class MainLift extends TargetMotor implements Lift { private final double speed = 1.0; + public static final double TOP= 31600; + + + private boolean auto=false; private int upPosition; private int downPosition; @@ -58,13 +63,35 @@ public void onAction() { }); // Lift Top - Control.mainLiftTop.setPressAction(new ButtonAction() { + /* Control.mainLiftTop.setPressAction(new ButtonAction() { @Override public void onAction() { if(encoder != null){ MainLift.super.setTarget(downPosition); } } + });*/ + Control.mainLiftAutoUp.setHeldAction(new ButtonAction() { + @Override + public void onAction() { + if(Hardware.liftEncoder.getInches().get() Date: Sat, 24 Mar 2018 09:53:16 -0400 Subject: [PATCH 08/38] Morning Commit --- .../frc/team555/robot/auto/DropCubeTop.java | 18 +++++++++++++++++ .../frc/team555/robot/auto/SwitchAuto.java | 2 +- .../frc/team555/robot/auto/TopCubeAuto.java | 20 +++++++++++++++++++ .../java/frc/team555/robot/core/Control.java | 17 +++++++++++++++- .../frc/team555/robot/core/PowerUpRobot.java | 9 +++++---- 5 files changed, 60 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/DropCubeTop.java create mode 100644 src/main/java/frc/team555/robot/auto/TopCubeAuto.java 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..25a9d3d --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/DropCubeTop.java @@ -0,0 +1,18 @@ +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.DriveTime; +import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; +import org.montclairrobotics.sprocket.states.StateMachine; + +public class DropCubeTop extends StateMachine { + public DropCubeTop(CubeIntake intake, GyroCorrection correction, Side side){ + super( + new SideTurn(correction, false, side), + // new MainLiftMove(12,.5,true), + new DriveTime(3, .9) + ); + } +} diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 84fc4c3..af02410 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -45,7 +45,7 @@ public static void loop(){ public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake, IntakeLift lift){ super(new ResetGyro(correction), new MoveLift(mainLift,MainLift.TOP*0.6,1,true), - new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction), + new DriveEncoderGyro(150, .25, Angle.ZERO, false, correction), new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide) ); } 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..64ff890 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java @@ -0,0 +1,20 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.components.MainLift; +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; + +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 DriveEncoderGyro(150, .1, Angle.ZERO, false, correction), + new ConditionalState(new DropCubeTop(intake, correction, SwitchAuto.startSidesChooser.getSelected()), SwitchAuto.startSide)); + } +} diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 91c5c2f..d003653 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -1,6 +1,7 @@ package frc.team555.robot.core; import edu.wpi.first.wpilibj.Joystick; +import org.montclairrobotics.sprocket.control.ArcadeDriveInput; import org.montclairrobotics.sprocket.control.Button; import org.montclairrobotics.sprocket.control.JoystickButton; import org.montclairrobotics.sprocket.control.SquaredDriveInput; @@ -55,7 +56,8 @@ public static void init() { auxStick = new Joystick(auxStickID); // Drive Input creation - driveInput = new SquaredDriveInput(driveStick); + //driveInput = new SquaredDriveInput(driveStick); + driveInput=new ArcadeDriveInput(driveStick); // ================== // Button Creation @@ -69,11 +71,24 @@ public static void init() { // intakeLiftDecrement = new JoystickButton(auxStick,12); // mainLiftTop = new JoystickButton(auxStick,9); // mainLiftBottom = new JoystickButton(auxStick,8); + + + + /* + OLD mainLiftManualUp = new JoystickButton(auxStick,3); mainLiftManualDown = new JoystickButton(auxStick,2); intakeLiftManualUp = new JoystickButton(auxStick,5); intakeLiftManualDown = new JoystickButton(auxStick,4); mainLiftAutoUp = new JoystickButton(auxStick, 1); + */ + mainLiftManualUp = new JoystickButton(auxStick,6); + mainLiftManualDown = new JoystickButton(auxStick,4); + intakeLiftManualUp = new JoystickButton(auxStick,5); + intakeLiftManualDown = new JoystickButton(auxStick,3); + mainLiftAutoUp = new JoystickButton(auxStick, 1); + + // intakeLiftTop = new JoystickButton(auxStick, 12); // intakeLiftBottom = new JoystickButton(auxStick, 13); // intakeSubroutine = new JoystickButton(auxStick, 14); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 88d9bf5..214f733 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -62,7 +62,7 @@ public void robotInit(){ intake = new CubeIntake(); mainLift=new MainLift(); intakeLift=new IntakeLift(); - SetIntakeLift.setLift(intakeLift); + //SetIntakeLift.setLift(intakeLift); modules[0] = new DriveModule(new XY(-1, 0), new XY(0, 1), @@ -237,9 +237,10 @@ public void onAction() { addAutoMode(baseLine); addAutoMode(centerBaseLineLeft); addAutoMode(centerBaseLineRight); - addAutoMode(new AutoMode("switch", new SwitchAuto(mainLift,correction, intake, intakeLift))); + addAutoMode(new AutoMode("Switch Using Intake", new SwitchAuto(mainLift,correction, intake, intakeLift))); addAutoMode(mainLiftUp); addAutoMode(turnQuarter); + addAutoMode(new AutoMode("Switch Using Lift", new TopCubeAuto(mainLift, intake, correction))); //addAutoMode(new AutoMode("Switch Auto", new SwitchAuto(correction, intake))); sendAutoModes(); @@ -306,8 +307,8 @@ public void update(){ SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); - Debug.msg("Main Lift Encoder",mainLift.encoder.getInches().get()); - Debug.msg("Intake Lift Encoder",intakeLift.encoder.getInches().get()); + SmartDashboard.putNumber("Main Lift Encoder",Hardware.liftEncoder.getInches().get()); + //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); gyroLocking(); SwitchAuto.loop(); } From 4ac054ee08eb8fb9759146598afbe43da9a1f870 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 24 Mar 2018 12:31:30 -0400 Subject: [PATCH 09/38] Around noon-it works --- src/main/java/frc/team555/robot/auto/DropCube.java | 3 ++- src/main/java/frc/team555/robot/auto/DropCubeTop.java | 5 +++-- src/main/java/frc/team555/robot/auto/SideTurn.java | 8 +++++--- src/main/java/frc/team555/robot/auto/SwitchAuto.java | 7 ++++++- src/main/java/frc/team555/robot/auto/SwitchAuto2.java | 3 ++- src/main/java/frc/team555/robot/auto/TopCubeAuto.java | 11 +++++++++-- src/main/java/frc/team555/robot/core/Control.java | 8 ++++---- .../java/frc/team555/robot/core/PowerUpRobot.java | 2 +- 8 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/DropCube.java b/src/main/java/frc/team555/robot/auto/DropCube.java index cc129ed..f4cf328 100644 --- a/src/main/java/frc/team555/robot/auto/DropCube.java +++ b/src/main/java/frc/team555/robot/auto/DropCube.java @@ -8,9 +8,10 @@ import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.states.State; import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.utils.Input; public class DropCube extends StateMachine{ - public DropCube(CubeIntake intake, GyroCorrection correction, Side side){ + public DropCube(CubeIntake intake, GyroCorrection correction, Input side){ super( new SideTurn(correction, false, side), // new MainLiftMove(12,.5,true), diff --git a/src/main/java/frc/team555/robot/auto/DropCubeTop.java b/src/main/java/frc/team555/robot/auto/DropCubeTop.java index 25a9d3d..326bda6 100644 --- a/src/main/java/frc/team555/robot/auto/DropCubeTop.java +++ b/src/main/java/frc/team555/robot/auto/DropCubeTop.java @@ -6,13 +6,14 @@ import org.montclairrobotics.sprocket.auto.states.DriveTime; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.utils.Input; public class DropCubeTop extends StateMachine { - public DropCubeTop(CubeIntake intake, GyroCorrection correction, Side side){ + public DropCubeTop(CubeIntake intake, GyroCorrection correction, Input side){ super( new SideTurn(correction, false, side), // new MainLiftMove(12,.5,true), - new DriveTime(3, .9) + new DriveTime(2, .75) ); } } diff --git a/src/main/java/frc/team555/robot/auto/SideTurn.java b/src/main/java/frc/team555/robot/auto/SideTurn.java index 434a3ef..5cc44cd 100644 --- a/src/main/java/frc/team555/robot/auto/SideTurn.java +++ b/src/main/java/frc/team555/robot/auto/SideTurn.java @@ -7,17 +7,19 @@ 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(); @@ -29,7 +31,7 @@ public void userStart() { } @Override public Angle getTgt(){ - if(side == Side.LEFT){ + if(side.get() == Side.LEFT){ return new Degrees(90); }else{ return new Degrees(-90); diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index af02410..8d7e269 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -46,7 +46,12 @@ public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intak super(new ResetGyro(correction), new MoveLift(mainLift,MainLift.TOP*0.6,1,true), new DriveEncoderGyro(150, .25, Angle.ZERO, false, correction), - new ConditionalState(new DropCube(intake, correction, startSidesChooser.getSelected()), startSide) + new ConditionalState(new DropCube(intake, correction, new Input(){ + @Override + public Side get() { + return startSidesChooser.getSelected(); + } + }), startSide) ); } } diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java index 3cb4ac0..28b382e 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java @@ -57,7 +57,8 @@ public static class RotateAndDropCube extends StateMachine { public RotateAndDropCube(GyroCorrection correction) { super( - new SideTurn(correction, false, startSidesChooser.getSelected()), + new SideTurn(correction, false, new Input(){ + public Side get(){return startSidesChooser.getSelected();}}), new DriveTime(3, .3)); //new MoveIntake(.8,0.5)); } diff --git a/src/main/java/frc/team555/robot/auto/TopCubeAuto.java b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java index 64ff890..95e30f6 100644 --- a/src/main/java/frc/team555/robot/auto/TopCubeAuto.java +++ b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java @@ -2,6 +2,7 @@ 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; @@ -9,12 +10,18 @@ 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 DriveEncoderGyro(150, .1, Angle.ZERO, false, correction), - new ConditionalState(new DropCubeTop(intake, correction, SwitchAuto.startSidesChooser.getSelected()), SwitchAuto.startSide)); + new DriveEncoderGyro(140, .5, Angle.ZERO, false, correction), + new ConditionalState(new DropCubeTop(intake, correction, new Input(){ + @Override + public Side get() { + return SwitchAuto.startSidesChooser.getSelected(); + } + }), SwitchAuto.startSide)); } } diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index d003653..a87d25d 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -82,10 +82,10 @@ public static void init() { intakeLiftManualDown = new JoystickButton(auxStick,4); mainLiftAutoUp = new JoystickButton(auxStick, 1); */ - mainLiftManualUp = new JoystickButton(auxStick,6); - mainLiftManualDown = new JoystickButton(auxStick,4); - intakeLiftManualUp = new JoystickButton(auxStick,5); - intakeLiftManualDown = new JoystickButton(auxStick,3); + mainLiftManualUp = new JoystickButton(auxStick,4); + mainLiftManualDown = new JoystickButton(auxStick,3); + intakeLiftManualUp = new JoystickButton(auxStick,6); + intakeLiftManualDown = new JoystickButton(auxStick,5); mainLiftAutoUp = new JoystickButton(auxStick, 1); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 214f733..2c8c4a8 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -307,7 +307,7 @@ public void update(){ SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); - SmartDashboard.putNumber("Main Lift Encoder",Hardware.liftEncoder.getInches().get()); + SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); gyroLocking(); SwitchAuto.loop(); From 05ced5a6a0cc05a03f513e3b4285a248809aea89 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 24 Mar 2018 12:48:45 -0400 Subject: [PATCH 10/38] Auto a bit safer hopefully --- src/main/java/frc/team555/robot/auto/DropCubeTop.java | 3 +++ src/main/java/frc/team555/robot/auto/TopCubeAuto.java | 10 ++++++++-- src/main/java/frc/team555/robot/core/PowerUpRobot.java | 8 ++++++++ 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/DropCubeTop.java b/src/main/java/frc/team555/robot/auto/DropCubeTop.java index 326bda6..f68d313 100644 --- a/src/main/java/frc/team555/robot/auto/DropCubeTop.java +++ b/src/main/java/frc/team555/robot/auto/DropCubeTop.java @@ -3,14 +3,17 @@ 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/TopCubeAuto.java b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java index 95e30f6..f728074 100644 --- a/src/main/java/frc/team555/robot/auto/TopCubeAuto.java +++ b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java @@ -16,12 +16,18 @@ 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 DriveEncoderGyro(140, .5, Angle.ZERO, false, correction), + new ConditionalState(new DropCubeTop(intake, correction, new Input(){ @Override public Side get() { return SwitchAuto.startSidesChooser.getSelected(); } - }), SwitchAuto.startSide)); + }), 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/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 2c8c4a8..fe97b26 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -1,5 +1,6 @@ package frc.team555.robot.core; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; @@ -307,12 +308,19 @@ public void update(){ SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); + 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",in.getInches().get()); gyroLocking(); SwitchAuto.loop(); } + private void debugCurrent(String name,WPI_TalonSRX motor) { + SmartDashboard.putNumber(name + " Current", motor.getOutputCurrent()); + } + private void gyroLocking(){ boolean autoLock = ((Math.abs(Control.driveInput.getTurn().toDegrees())<10) && (Math.abs(Control.driveInput.getDir().getY())>0.5)); From 32bac1deaf3c6de860e4e95d0825dd55dfa50443 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 24 Mar 2018 16:32:31 -0400 Subject: [PATCH 11/38] Point at which everything is good-4:30 --- .../java/frc/team555/robot/core/Control.java | 6 ++-- .../frc/team555/robot/core/PowerUpRobot.java | 35 ++++++++++++++----- .../robot/test/PowerUpRobotWithLift.java | 10 +++--- .../frc/team555/robot/utils/CoastMotor.java | 17 +++++++++ 4 files changed, 52 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/team555/robot/utils/CoastMotor.java diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index a87d25d..f2f8572 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -18,6 +18,7 @@ public class Control { // Drive Inputs public static DTInput driveInput; public static Button lock, + halfSpeed, //intakeLiftIncrement, //intakeLiftDecrement, intakeLiftManualUp, @@ -39,8 +40,8 @@ public class Control { //intakeSubroutine; - public static int FieldCentricID=2; - public static int ResetID=3; + //public static int FieldCentricID=2; + //public static int ResetID=3; // Joystick Declaration public static Joystick driveStick; public static Joystick auxStick; @@ -87,6 +88,7 @@ public static void init() { intakeLiftManualUp = new JoystickButton(auxStick,6); intakeLiftManualDown = new JoystickButton(auxStick,5); mainLiftAutoUp = new JoystickButton(auxStick, 1); + halfSpeed=new JoystickButton(driveStick,2); // intakeLiftTop = new JoystickButton(auxStick, 12); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index fe97b26..d9e1b70 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -6,6 +6,7 @@ import frc.team555.robot.components.CubeIntake; import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; +import frc.team555.robot.utils.CoastMotor; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; @@ -31,6 +32,7 @@ public class PowerUpRobot extends SprocketRobot { DriveTrain driveTrain; GyroCorrection correction; + Sensitivity sensitivity; GyroLock lock; boolean manualLock; CubeIntake intake; @@ -70,16 +72,16 @@ public void robotInit(){ 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)); @@ -109,9 +111,10 @@ public void robotInit(){ ArrayList> steps = new ArrayList<>(); correction = new GyroCorrection(Hardware.navx, new PID(1.5, 0, 0.0015), 90, 1); + sensitivity=new Sensitivity(1,0.6); lock = new GyroLock(correction); steps.add(new Deadzone()); - steps.add(new Sensitivity(1)); + steps.add(sensitivity); steps.add(correction); driveTrain.setPipeline(new DTPipeline(steps)); @@ -136,15 +139,29 @@ 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() { + @Override + public void onAction() { + sensitivity.set(1,0.6); + } }); 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/utils/CoastMotor.java b/src/main/java/frc/team555/robot/utils/CoastMotor.java new file mode 100644 index 0000000..c210c2f --- /dev/null +++ b/src/main/java/frc/team555/robot/utils/CoastMotor.java @@ -0,0 +1,17 @@ +package frc.team555.robot.utils; + +import edu.wpi.first.wpilibj.SpeedController; +import org.montclairrobotics.sprocket.motors.Motor; +import org.montclairrobotics.sprocket.utils.Utils; + +public class CoastMotor extends Motor{ + public CoastMotor(SpeedController motor, boolean brakeMode) { + super(motor, brakeMode); + } + + public void set(double power) { + + super.getMotor().set(power); + + } +} From 733a375de539128aebba40ae1cbccd0c9253fc60 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 24 Mar 2018 18:37:25 -0400 Subject: [PATCH 12/38] End of day, everything ok. --- .../java/frc/team555/robot/auto/DropCube.java | 8 ++++++-- .../java/frc/team555/robot/auto/SwitchAuto.java | 15 +++++++++------ .../frc/team555/robot/components/MainLift.java | 7 ++++++- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/DropCube.java b/src/main/java/frc/team555/robot/auto/DropCube.java index f4cf328..6139c03 100644 --- a/src/main/java/frc/team555/robot/auto/DropCube.java +++ b/src/main/java/frc/team555/robot/auto/DropCube.java @@ -4,18 +4,22 @@ 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.State; import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; public class DropCube extends StateMachine{ - public DropCube(CubeIntake intake, GyroCorrection correction, Input side){ + public DropCube(MainLift mainLift,CubeIntake intake, GyroCorrection correction, Input side){ super( + new MoveLift(mainLift,MainLift.TOP*0.6,1,true), + new DriveEncoderGyro(140, .5, Angle.ZERO, false, correction), new SideTurn(correction, false, side), // new MainLiftMove(12,.5,true), - new DriveTime(3, .3), + new DriveTime(2, .5), new AutoState() { @Override public void stateUpdate() { diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 8d7e269..8dc9751 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -44,14 +44,17 @@ public static void loop(){ public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake, IntakeLift lift){ super(new ResetGyro(correction), - new MoveLift(mainLift,MainLift.TOP*0.6,1,true), - new DriveEncoderGyro(150, .25, Angle.ZERO, false, correction), - new ConditionalState(new DropCube(intake, correction, new Input(){ + new ConditionalState(new DropCube(mainLift,intake, correction, new Input(){ @Override public Side get() { - return startSidesChooser.getSelected(); + return SwitchAuto.startSidesChooser.getSelected(); } - }), startSide) - ); + }), 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/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 0ad021c..4d47009 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -47,7 +47,12 @@ public void onAction() { @Override public void onAction() { if(!Hardware.liftLimitSwitch.get()) { - set(-speed); + if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(11)) { + set(-speed); + } + else { + set(-0.4); + } }else{ set(0); encoder.reset(); From 2b1da8e37d8b1e24675ed493a811f8eae5e8f31d Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 24 Mar 2018 18:46:07 -0400 Subject: [PATCH 13/38] More power, does it in time? --- src/main/java/frc/team555/robot/auto/DropCube.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/DropCube.java b/src/main/java/frc/team555/robot/auto/DropCube.java index 6139c03..95beccd 100644 --- a/src/main/java/frc/team555/robot/auto/DropCube.java +++ b/src/main/java/frc/team555/robot/auto/DropCube.java @@ -8,6 +8,7 @@ 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; @@ -15,11 +16,13 @@ public class DropCube extends StateMachine{ public DropCube(MainLift mainLift,CubeIntake intake, GyroCorrection correction, Input side){ super( - new MoveLift(mainLift,MainLift.TOP*0.6,1,true), - new DriveEncoderGyro(140, .5, Angle.ZERO, false, correction), + new MultiState( + new MoveLift(mainLift,MainLift.TOP*0.6,1,true), + new DriveEncoderGyro(140, .7, Angle.ZERO, false, correction) + ), new SideTurn(correction, false, side), // new MainLiftMove(12,.5,true), - new DriveTime(2, .5), + new DriveTime(1, .5), new AutoState() { @Override public void stateUpdate() { @@ -28,7 +31,7 @@ public void stateUpdate() { @Override public boolean isDone() { - return super.timeInState()>2; + return super.timeInState()>4; } } ); From 7d29d7c406a9faf88bc1e07ec40b98a6be778ec5 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sun, 25 Mar 2018 12:36:13 -0400 Subject: [PATCH 14/38] end of alliance selection --- src/main/java/frc/team555/robot/core/Hardware.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 4dc1eb3..4bbabdb 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -142,7 +142,7 @@ public static void init(){ //motorIntakeClamp = new WPI_TalonSRX(DeviceID.motorIntakeClamp); - intakeClosedSwitch = new DigitalInput(9); + intakeClosedSwitch = new DigitalInput(6); intakeOpenSwitch = new DigitalInput(8); @@ -175,6 +175,6 @@ public static void init(){ navx = new NavXInput(DeviceID.navxPort); - liftLimitSwitch = new DigitalInput(6); + liftLimitSwitch = new DigitalInput(9); } } From 886e6a5d326bbfe4cff4790478d2b0afec404884 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sun, 25 Mar 2018 13:37:52 -0400 Subject: [PATCH 15/38] Auto Lift --- .../team555/robot/auto/AutoClimbSequence.java | 42 +++++++++++++++++++ .../team555/robot/components/MainLift.java | 12 +++++- .../java/frc/team555/robot/core/Control.java | 3 +- .../frc/team555/robot/core/PowerUpRobot.java | 23 ++++++++-- 4 files changed, 73 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/AutoClimbSequence.java 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..f2a87ac --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java @@ -0,0 +1,42 @@ +package frc.team555.robot.auto; + +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.StateMachine; + +import static frc.team555.robot.components.MainLift.TOP; + +public class AutoClimbSequence extends StateMachine{ + + 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) + + + ); + + } + + + + +} diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 4d47009..d236eb2 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -1,12 +1,20 @@ package frc.team555.robot.components; +import frc.team555.robot.auto.MoveLift; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; +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.geometry.Angle; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; +import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.PID; import java.nio.channels.Pipe; @@ -14,7 +22,7 @@ public class MainLift extends TargetMotor implements Lift { private final double speed = 1.0; - public static final double TOP= 31600; + public static final double TOP= 30834.0; private boolean auto=false; @@ -31,7 +39,7 @@ public MainLift(){ Control.mainLiftManualUp.setPressAction(new ButtonAction() { @Override public void onAction() { - set(speed); + set(speed); } }); diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index f2f8572..8598f75 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -27,7 +27,7 @@ public class Control { //intakeLiftBottom, mainLiftAutoUp, mainLiftManualUp, - mainLiftManualDown; + mainLiftManualDown, // mainLiftTop, // mainLiftBottom, //openButton, @@ -38,6 +38,7 @@ public class Control { //intakeRotateUpManual, //intakeRotateDownManual; //intakeSubroutine; + autoClimb; //public static int FieldCentricID=2; diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index d9e1b70..36ba1e0 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -1,6 +1,8 @@ package frc.team555.robot.core; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; @@ -8,6 +10,7 @@ import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.CoastMotor; import frc.team555.robot.utils.Side; +import javafx.scene.Camera; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; import org.montclairrobotics.sprocket.auto.states.*; @@ -31,7 +34,7 @@ public class PowerUpRobot extends SprocketRobot { DriveTrain driveTrain; - GyroCorrection correction; + public static GyroCorrection correction; Sensitivity sensitivity; GyroLock lock; boolean manualLock; @@ -44,6 +47,7 @@ public class PowerUpRobot extends SprocketRobot { 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; @@ -53,7 +57,7 @@ public class PowerUpRobot extends SprocketRobot { @Override public void robotInit(){ - + CameraServer.getInstance().startAutomaticCapture(); DriveEncoders.TOLLERANCE=/*45.5363/17.1859*/6; TurnGyro.TURN_SPEED=0.3; TurnGyro.tolerance=new Degrees(3); @@ -150,7 +154,7 @@ public void onAction() { } });*/ - Control.halfSpeed.setPressAction(new ButtonAction() { + /*Control.halfSpeed.setPressAction(new ButtonAction() { @Override public void onAction() { sensitivity.set(0.5,0.3); @@ -163,7 +167,7 @@ public void onAction() { sensitivity.set(1,0.6); } }); - +*/ @@ -252,6 +256,8 @@ public void onAction() { 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); @@ -290,6 +296,7 @@ public void onAction() { */ // vision stuff +// CameraServer.getInstance().startAutomaticCapture(); /*UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); camera.setResolution(IMG_WIDTH, IMG_HEIGHT); @@ -347,6 +354,14 @@ 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(0.4,0.3); + } + else + { + sensitivity.set(1,0.6); + } } @Override From c13d2707ae7bba33f3f8a3a61a8116fb80e02d7c Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sun, 25 Mar 2018 15:07:57 -0400 Subject: [PATCH 16/38] Added auto climb button --- .../team555/robot/auto/AutoClimbSequence.java | 45 ++++++++++++++++--- .../team555/robot/components/MainLift.java | 2 +- .../java/frc/team555/robot/core/Control.java | 1 + .../frc/team555/robot/core/PowerUpRobot.java | 26 ++++++++++- 4 files changed, 66 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java index f2a87ac..d92bcd9 100644 --- a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java +++ b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java @@ -1,42 +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{ +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, + 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) - + 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/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index d236eb2..990a9fd 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -55,7 +55,7 @@ public void onAction() { @Override public void onAction() { if(!Hardware.liftLimitSwitch.get()) { - if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(11)) { + if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); } else { diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 8598f75..92b9227 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -90,6 +90,7 @@ public static void init() { intakeLiftManualDown = new JoystickButton(auxStick,5); mainLiftAutoUp = new JoystickButton(auxStick, 1); halfSpeed=new JoystickButton(driveStick,2); + autoClimb = new JoystickButton(auxStick, 2); // intakeLiftTop = new JoystickButton(auxStick, 12); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 36ba1e0..cd66d11 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -41,6 +41,7 @@ public class PowerUpRobot extends SprocketRobot { CubeIntake intake; MainLift mainLift; IntakeLift intakeLift; + StateMachine autoClimb; //vision stuff private static final int IMG_WIDTH = 320; @@ -69,6 +70,8 @@ public void robotInit(){ 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), @@ -113,8 +116,6 @@ public void robotInit(){ new DashboardInput("auto Selection"); ArrayList> steps = new ArrayList<>(); - - correction = new GyroCorrection(Hardware.navx, new PID(1.5, 0, 0.0015), 90, 1); sensitivity=new Sensitivity(1,0.6); lock = new GyroLock(correction); steps.add(new Deadzone()); @@ -295,6 +296,27 @@ public void onAction() { }); */ + 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(); From 458f11b770974ddb37086a1d93959bf922fa0e6c Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Mon, 26 Mar 2018 09:03:55 -0400 Subject: [PATCH 17/38] Removed extraneous import Was causing Jenkins to fail --- src/main/java/frc/team555/robot/core/PowerUpRobot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index cd66d11..bf2aa72 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -10,7 +10,6 @@ import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.CoastMotor; import frc.team555.robot.utils.Side; -import javafx.scene.Camera; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; import org.montclairrobotics.sprocket.auto.states.*; From c03593298e4b9be9bd639138330ac6902ac2490c Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Tue, 10 Apr 2018 15:25:53 -0400 Subject: [PATCH 18/38] Created AutoSwitch.java and helper default classes --- .../frc/team555/robot/auto/AutoSwitch.java | 19 +++++++++++++++++++ .../frc/team555/robot/auto/AutoSwitchLR.java | 12 ++++++++++++ .../frc/team555/robot/auto/AutoSwitchMid.java | 11 +++++++++++ 3 files changed, 42 insertions(+) create mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitch.java create mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitchLR.java create mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitchMid.java 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..74576ee --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitch.java @@ -0,0 +1,19 @@ +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 { + public AutoSwitch(String name, State... states) { + super(name, states); + } + + public static AutoSwitch fromMiddle(Side target) { + return new AutoSwitchMid(Side.fromDriverStation()[0]); + } + + public static AutoSwitch fromSide(Side pos, Side target) { + return new AutoSwitchLR(pos, target); + } +} 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..367b413 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -0,0 +1,12 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.states.State; + +class AutoSwitchLR extends AutoSwitch { + AutoSwitchLR(Side pos, Side target) { + super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")", + + ); + } +} 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..064ebbb --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -0,0 +1,11 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.states.State; + +class AutoSwitchMid extends AutoSwitch { + AutoSwitchMid(Side target) { + super("Cube to " + target.toString() + " Switch (M)", + ); + } +} From b55fc1d5fb8262e36819fb63feef703de1ee7894 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Tue, 10 Apr 2018 16:06:01 -0400 Subject: [PATCH 19/38] Completed AutoSwitch.fromMiddle(), awaiting testing --- src/main/java/frc/team555/robot/Robot.java | 6 - .../frc/team555/robot/auto/AutoSwitch.java | 6 +- .../frc/team555/robot/auto/AutoSwitchLR.java | 2 +- .../frc/team555/robot/auto/AutoSwitchMid.java | 23 +- .../team555/robot/auto/DropCubeSwitch.java | 14 +- .../frc/team555/robot/auto/SwitchAuto.java | 2 +- .../frc/team555/robot/auto/SwitchAuto2.java | 1 + .../frc/team555/robot/core/PowerUpRobot.java | 4 +- .../frc/team555/robot/core/PowerUpRobot2.java | 502 ------------------ 9 files changed, 37 insertions(+), 523 deletions(-) delete mode 100644 src/main/java/frc/team555/robot/core/PowerUpRobot2.java diff --git a/src/main/java/frc/team555/robot/Robot.java b/src/main/java/frc/team555/robot/Robot.java index b1531a9..74297ab 100644 --- a/src/main/java/frc/team555/robot/Robot.java +++ b/src/main/java/frc/team555/robot/Robot.java @@ -1,12 +1,6 @@ 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 { } diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitch.java b/src/main/java/frc/team555/robot/auto/AutoSwitch.java index 74576ee..44325b4 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitch.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitch.java @@ -9,11 +9,11 @@ public AutoSwitch(String name, State... states) { super(name, states); } - public static AutoSwitch fromMiddle(Side target) { + public static AutoSwitch fromMiddle() { return new AutoSwitchMid(Side.fromDriverStation()[0]); } - public static AutoSwitch fromSide(Side pos, Side target) { - return new AutoSwitchLR(pos, target); + public static AutoSwitch fromSide(Side pos) { + return new AutoSwitchLR(pos, Side.fromDriverStation()[0]); } } diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java index 367b413..64e2c28 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -7,6 +7,6 @@ class AutoSwitchLR extends AutoSwitch { AutoSwitchLR(Side pos, Side target) { super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")", - ); + ); } } diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java index 064ebbb..edb595c 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -1,11 +1,30 @@ package frc.team555.robot.auto; +import frc.team555.robot.components.IntakeLift; +import frc.team555.robot.core.Hardware; +import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; -import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderLock; +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.geometry.Angle; 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, 0.75, Angle.ZERO, true, PowerUpRobot.correction), + /* Turn π/2 radians (in the correct directon) */ + new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * 0.75, Angle.QUARTER, true, PowerUpRobot.correction), + /* Drive forward 36 inches */ + new DriveEncoderGyro(36, 0.75, Angle.ZERO, true, PowerUpRobot.correction), + /* Move intake lift up 8 inches */ + new IntakeLiftMove(8, 0.75, true), + /* Turn π/2 radians (in the opposite direction) */ + new DropCubeSwitch(target, PowerUpRobot.correction) + ); } } diff --git a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java index 702ad22..d4c53dc 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; @@ -15,12 +16,13 @@ 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(Side 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/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 2371087..ffc550a 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -13,7 +13,7 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; - +@Deprecated public class SwitchAuto extends StateMachine{ static Input startSide; diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto2.java b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java index 86aabe6..d0e8080 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; diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 0a297d3..4b83bbf 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -30,10 +30,10 @@ public class PowerUpRobot extends SprocketRobot { DriveTrain driveTrain; - GyroCorrection correction; + public static GyroCorrection correction; GyroLock lock; boolean manualLock; - CubeIntake intake; + public static CubeIntake intake; public static SendableChooser startSidesChooser; 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(); - } -} From 375a36c8f0d396a264fd310995f5ef758943d6eb Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Wed, 18 Apr 2018 09:21:40 -0400 Subject: [PATCH 20/38] AutoSwitch.java finished! Ready to test. --- .../frc/team555/robot/auto/AutoSwitch.java | 2 ++ .../frc/team555/robot/auto/AutoSwitchLR.java | 24 +++++++++++++++++-- .../frc/team555/robot/auto/AutoSwitchMid.java | 13 ++++------ .../frc/team555/robot/auto/OldSideAuto.java | 2 +- 4 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitch.java b/src/main/java/frc/team555/robot/auto/AutoSwitch.java index 44325b4..2a9236f 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitch.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitch.java @@ -5,6 +5,8 @@ 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); } diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java index 64e2c28..92837a5 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -1,12 +1,32 @@ package frc.team555.robot.auto; +import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; -import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.auto.states.DriveEncoderGyro; +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.geometry.Angle; 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(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 index edb595c..1325a11 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -1,11 +1,8 @@ package frc.team555.robot.auto; -import frc.team555.robot.components.IntakeLift; -import frc.team555.robot.core.Hardware; 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.DriveEncoderLock; import org.montclairrobotics.sprocket.auto.states.ResetGyro; import org.montclairrobotics.sprocket.geometry.Angle; @@ -16,14 +13,12 @@ class AutoSwitchMid extends AutoSwitch { /* Reset gyro */ new ResetGyro(PowerUpRobot.correction), /* Drive forward 12 inches */ - new DriveEncoderGyro(12, 0.75, Angle.ZERO, true, PowerUpRobot.correction), + new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction), /* Turn π/2 radians (in the correct directon) */ - new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * 0.75, Angle.QUARTER, true, PowerUpRobot.correction), + new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * POWER, Angle.QUARTER, true, PowerUpRobot.correction), /* Drive forward 36 inches */ - new DriveEncoderGyro(36, 0.75, Angle.ZERO, true, PowerUpRobot.correction), - /* Move intake lift up 8 inches */ - new IntakeLiftMove(8, 0.75, true), - /* Turn π/2 radians (in the opposite direction) */ + new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), + /* Move the lift, turn, and drop the cube into the switch */ new DropCubeSwitch(target, PowerUpRobot.correction) ); } diff --git a/src/main/java/frc/team555/robot/auto/OldSideAuto.java b/src/main/java/frc/team555/robot/auto/OldSideAuto.java index 98bdffb..1550c71 100644 --- a/src/main/java/frc/team555/robot/auto/OldSideAuto.java +++ b/src/main/java/frc/team555/robot/auto/OldSideAuto.java @@ -63,7 +63,7 @@ public void start() { if(Side.fromDriverStation()[0] == PowerUpRobot.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(Side.fromDriverStation()[0], correction)); Debug.msg("Target", "RightSwitch"); }else if(crossover.get()){ states.add(new DriveEncoderGyro(100, .75, Angle.ZERO, false, correction)); From 5d54d05487e29bdc2cabd1be43e64b5723d14495 Mon Sep 17 00:00:00 2001 From: Joshua Rapoport Date: Wed, 18 Apr 2018 18:13:47 -0400 Subject: [PATCH 21/38] AutoSwitch.java declared in PowerUpRobot. --- src/main/java/frc/team555/robot/core/PowerUpRobot.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 4b83bbf..97cad83 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -259,6 +259,15 @@ public void onAction() { 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 Auto", new SwitchAuto(correction, intake))); sendAutoModes(); From 99046e84a24d0604e468dea46e6dc693f28ba3ec Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Wed, 18 Apr 2018 18:40:42 -0400 Subject: [PATCH 22/38] Intake Working --- .../frc/team555/robot/auto/AutoStart.java | 11 ++++++++ .../team555/robot/auto/SetIntakeRotation.java | 3 +- .../frc/team555/robot/auto/SwitchAuto.java | 3 +- .../team555/robot/components/CubeIntake.java | 26 ++++++++++------- .../frc/team555/robot/core/PowerUpRobot.java | 28 +++++++++++++++---- .../frc/team555/robot/test/TestRobot.java | 2 ++ .../frc/team555/robot/test/TestRobot2.java | 18 ++++++++++-- 7 files changed, 70 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/AutoStart.java 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..23865fe --- /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; + +public class AutoStart extends StateMachine { + public AutoStart(GyroCorrection correction){ + super(new ResetGyro(correction)); + } +} diff --git a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java index 11b7a64..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.roationalMotor.setTarget(position); + // intake.roationalMotor.setTarget(position); } @Override diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 8dc9751..2c76020 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -5,6 +5,7 @@ import frc.team555.robot.components.CubeIntake; import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; +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; @@ -42,7 +43,7 @@ public static void loop(){ SmartDashboard.putBoolean("Correct Side", startSide.get()); } - public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake, IntakeLift lift){ + public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake){ super(new ResetGyro(correction), new ConditionalState(new DropCube(mainLift,intake, correction, new Input(){ @Override diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 918fb70..9d55565 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -24,7 +24,7 @@ public class CubeIntake implements Updatable, Togglable{ public final Motor left; public final Motor right; - public final TargetMotor roationalMotor; + // public final TargetMotor roationalMotor; public final double tolerance = 1; // Todo: Maybe needs to be tuned public final int upPos = 0; @@ -39,21 +39,21 @@ public CubeIntake() { this.right = new Motor(Hardware.motorIntakeR); right.setInverted(true); //this.clamp = new Motor(Hardware.motorIntakeClamp); - Motor rotateMotor=new Motor(Hardware.motorIntakeRotate); - this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented + //Motor rotateMotor=new Motor(Hardware.motorIntakeRotate); + // this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented this.power = new Input() { @Override public Vector get() { double x=-Control.auxStick.getX(); - if(Math.abs(x)<0.2) + if(Math.abs(x)<0.1) { x=0; } double y=-Control.auxStick.getY(); - if(Math.abs(y)<0.2) + if(Math.abs(y)<0.1) { - y=0; + y=-.05; } return new XY(x,y); } @@ -107,7 +107,7 @@ public void onAction() { } }); */ - roationalMotor.setPower(0); + // roationalMotor.setPower(0); Updater.add(this, Priority.CALC); } @@ -115,10 +115,9 @@ public void onAction() { @Override public void update() { Vector p = power.get(); - if(!auto) { - left.set(p.getY() - p.getX()); - right.set(p.getY() + p.getX()); + left.set(p.getY() - p.getX()*.25); + right.set(p.getY() + p.getX()*.25); } auto=false; } @@ -129,6 +128,13 @@ 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(1); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index bf2aa72..d45d0ce 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -3,6 +3,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; @@ -39,8 +40,12 @@ public class PowerUpRobot extends SprocketRobot { boolean manualLock; CubeIntake intake; MainLift mainLift; - IntakeLift intakeLift; + // IntakeLift intakeLift; StateMachine autoClimb; + public static Side startSide; + SendableChooser startSideChooser; + static Input switchOnSide; + //vision stuff private static final int IMG_WIDTH = 320; @@ -57,6 +62,17 @@ 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; @@ -68,7 +84,6 @@ public void robotInit(){ 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); @@ -261,7 +276,7 @@ public void onAction() { addAutoMode(baseLine); addAutoMode(centerBaseLineLeft); addAutoMode(centerBaseLineRight); - addAutoMode(new AutoMode("Switch Using Intake", new SwitchAuto(mainLift,correction, intake, intakeLift))); + 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))); @@ -330,7 +345,7 @@ public void onAction() { } }); visionThread.start();*/ - intakeLift.setPower(0); + // intakeLift.setPower(0); } @Override @@ -359,7 +374,7 @@ public void update(){ SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); gyroLocking(); - SwitchAuto.loop(); + startSide = startSideChooser.getSelected(); } private void debugCurrent(String name,WPI_TalonSRX motor) { @@ -387,6 +402,7 @@ private void gyroLocking(){ @Override public void userDisabledPeriodic(){ - SwitchAuto.disabled(); + SmartDashboard.putData(startSideChooser); + startSide = startSideChooser.getSelected(); } } 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 1f92227..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.motorIntakeR, input); - ControlledMotor testMotor2 = new ControlledMotor(Hardware.motorIntakeL, 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(); From 17adf1f509cefe65daee9f7d59a15a26480d924b Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Wed, 18 Apr 2018 19:02:33 -0400 Subject: [PATCH 23/38] Fix error Fix errors --- src/main/java/frc/team555/robot/auto/AutoSwitchLR.java | 2 +- src/main/java/frc/team555/robot/auto/OldSideAuto.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java index 64e2c28..2a23ac5 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -5,7 +5,7 @@ class AutoSwitchLR extends AutoSwitch { AutoSwitchLR(Side pos, Side target) { - super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")", + super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")" ); } diff --git a/src/main/java/frc/team555/robot/auto/OldSideAuto.java b/src/main/java/frc/team555/robot/auto/OldSideAuto.java index 98bdffb..1550c71 100644 --- a/src/main/java/frc/team555/robot/auto/OldSideAuto.java +++ b/src/main/java/frc/team555/robot/auto/OldSideAuto.java @@ -63,7 +63,7 @@ public void start() { if(Side.fromDriverStation()[0] == PowerUpRobot.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(Side.fromDriverStation()[0], correction)); Debug.msg("Target", "RightSwitch"); }else if(crossover.get()){ states.add(new DriveEncoderGyro(100, .75, Angle.ZERO, false, correction)); From 93acdb842d59b3cb40715566a3405cf3eb34b50b Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Wed, 18 Apr 2018 20:21:10 -0400 Subject: [PATCH 24/38] It runs on the robot again --- .../frc/team555/robot/auto/SwitchAuto.java | 2 +- .../team555/robot/components/MainLift.java | 4 ++-- .../frc/team555/robot/core/PowerUpRobot.java | 19 ++++++++++--------- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 2c76020..e4a5e15 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -26,7 +26,7 @@ public static void init(){ for(Side side : Side.values()){ startSidesChooser.addObject(side.toString(), side); } - SmartDashboard.putData(startSidesChooser); + //SmartDashboard.putData(startSidesChooser); startSide = new Input() { @Override public Boolean get() { diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 990a9fd..3c65752 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -23,7 +23,7 @@ public class MainLift extends TargetMotor implements Lift { private final double speed = 1.0; public static final double TOP= 30834.0; - + private final boolean LIMIT_SWITCH_DISABLED=true; private boolean auto=false; @@ -54,7 +54,7 @@ public void onAction() { Control.mainLiftManualDown.setHeldAction(new ButtonAction() { @Override public void onAction() { - if(!Hardware.liftLimitSwitch.get()) { + if(LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) { if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); } diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index d45d0ce..9607c91 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -43,7 +43,7 @@ public class PowerUpRobot extends SprocketRobot { // IntakeLift intakeLift; StateMachine autoClimb; public static Side startSide; - SendableChooser startSideChooser; + //SendableChooser startSideChooser; static Input switchOnSide; @@ -62,15 +62,15 @@ 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() { + //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(); @@ -374,7 +374,7 @@ public void update(){ SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); gyroLocking(); - startSide = startSideChooser.getSelected(); + //startSide = startSideChooser.getSelected(); } private void debugCurrent(String name,WPI_TalonSRX motor) { @@ -402,7 +402,8 @@ private void gyroLocking(){ @Override public void userDisabledPeriodic(){ - SmartDashboard.putData(startSideChooser); - startSide = startSideChooser.getSelected(); + //SmartDashboard.putData(startSideChooser); + //startSide = startSideChooser.getSelected(); + SwitchAuto.disabled(); } } From cf56f9438a87a9f2e28bfe65e3dd473cf73327fb Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 12:49:24 -0400 Subject: [PATCH 25/38] Cleanup, re-enabled roational motor but with the right name --- .../robot/auto/GRIP/YellowPipeline.java | 248 ++++++++++++++++++ .../team555/robot/components/CubeIntake.java | 40 +-- .../java/frc/team555/robot/core/Control.java | 21 +- .../java/frc/team555/robot/core/Hardware.java | 4 + .../frc/team555/robot/core/PowerUpRobot.java | 34 +++ .../frc/team555/robot/utils/LimitedMotor.java | 31 +++ .../frc/team555/robot/utils/POVButton.java | 21 ++ 7 files changed, 373 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/GRIP/YellowPipeline.java create mode 100644 src/main/java/frc/team555/robot/utils/LimitedMotor.java create mode 100644 src/main/java/frc/team555/robot/utils/POVButton.java 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/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 9d55565..bcbc962 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -1,8 +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,6 +13,7 @@ import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.motors.Motor; +import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.Togglable; @@ -18,13 +21,13 @@ public class CubeIntake implements Updatable, Togglable{ - private double rotatePower=1; + private double rotatePower=.1; private boolean auto; - public final Motor left; - public final Motor right; - // public final TargetMotor roationalMotor; + 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; @@ -32,14 +35,16 @@ public class CubeIntake implements Updatable, Togglable{ public final int middlePos = 1; - public final Input power; + public Input power; public CubeIntake() { this.left = new Motor(Hardware.motorIntakeL); this.right = new Motor(Hardware.motorIntakeR); right.setInverted(true); //this.clamp = new Motor(Hardware.motorIntakeClamp); - //Motor rotateMotor=new Motor(Hardware.motorIntakeRotate); + LimitedMotor rotateMotor=new LimitedMotor(Hardware.motorIntakeRotate,true,Hardware.intakeLowerBound,Hardware.intakeUpperBound); + //Motor motor=new Motor(Hardware.motorIntakeRotate); + this.rotate=new TargetMotor(Hardware.intakeRotationEncoder,new BangBang(tolerance,rotatePower),rotateMotor); // this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented this.power = new Input() { @@ -60,23 +65,24 @@ public Vector get() { }; - /*Control.intakeRotateDown.setPressAction(new ButtonAction() { + Control.intakeRotateDown.setPressAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.set(downPos); + rotate.setTarget(downPos); + SmartDashboard.putString("Rotating Down","DOWN"); } }); Control.intakeRotateUp.setPressAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.set(upPos); + rotate.setTarget(upPos); } }); Control.intakeRotateMiddle.setPressAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.set(middlePos); + rotate.setTarget(middlePos); } }); @@ -84,30 +90,30 @@ public void onAction() { Control.intakeRotateUpManual.setPressAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.setPower(rotatePower); + rotate.setPower(rotatePower); } }); Control.intakeRotateUpManual.setReleaseAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.setPower(0); + rotate.setPower(0); } }); Control.intakeRotateDownManual.setPressAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.setPower(-rotatePower); + rotate.setPower(-rotatePower); } }); Control.intakeRotateDownManual.setReleaseAction(new ButtonAction() { @Override public void onAction() { - roationalMotor.setPower(0); + rotate.setPower(0); } }); -*/ - // roationalMotor.setPower(0); + + rotate.setPower(0); Updater.add(this, Priority.CALC); } @@ -120,6 +126,8 @@ public void update() { right.set(p.getY() + p.getX()*.25); } auto=false; + Debug.msg("RotateMotor PID OUT",rotate.pid.get()); + Debug.msg("RotateMotor PID TGT",rotate.pid.getTarget()); } diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 92b9227..28b1c3b 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -1,6 +1,7 @@ package frc.team555.robot.core; import edu.wpi.first.wpilibj.Joystick; +import frc.team555.robot.utils.POVButton; import org.montclairrobotics.sprocket.control.ArcadeDriveInput; import org.montclairrobotics.sprocket.control.Button; import org.montclairrobotics.sprocket.control.JoystickButton; @@ -32,11 +33,11 @@ public class Control { // mainLiftBottom, //openButton, //closeButton, - //intakeRotateUp, - //intakeRotateDown, - //intakeRotateMiddle, - //intakeRotateUpManual, - //intakeRotateDownManual; + intakeRotateUp, + intakeRotateDown, + intakeRotateMiddle, + intakeRotateUpManual, + intakeRotateDownManual, //intakeSubroutine; autoClimb; @@ -102,11 +103,11 @@ public static void init() { // closeButton=new JoystickButton(Control.auxStick,9); //Intake Buttons - // intakeRotateUp = new JoystickButton(auxStick, 12); - // intakeRotateDown = new JoystickButton(auxStick, 4); - // intakeRotateMiddle=new JoystickButton(auxStick,5); - // intakeRotateUpManual = new JoystickButton(auxStick, 3); - // intakeRotateDownManual = new JoystickButton(auxStick, 2); + intakeRotateUp = new POVButton(auxStick, 0); + intakeRotateDown = new POVButton(auxStick, 180); + intakeRotateMiddle=new POVButton(auxStick,90); + intakeRotateUpManual = new JoystickButton(auxStick, 7); + intakeRotateDownManual = new JoystickButton(auxStick, 11); // Auxiliary Buttons diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 4bbabdb..83d72f4 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -103,6 +103,8 @@ private static class DeviceID { public static DigitalInput intakeClosedSwitch; public static DigitalInput intakeOpenSwitch; + public static DigitalInput intakeUpperBound; + public static DigitalInput intakeLowerBound; // Encoders @@ -145,6 +147,8 @@ public static void init(){ intakeClosedSwitch = new DigitalInput(6); intakeOpenSwitch = new DigitalInput(8); + intakeUpperBound=new DigitalInput(5); + intakeLowerBound=new DigitalInput(9); //double ticksPerInch=6544.0/143.0;` //old/new=17.1859 * 1.25/(6544.0/143.0) diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 9607c91..80d6cb0 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -127,14 +127,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<>(); sensitivity=new Sensitivity(1,0.6); lock = new GyroLock(correction); steps.add(new Deadzone()); steps.add(sensitivity); steps.add(correction); + steps.add(vision); driveTrain.setPipeline(new DTPipeline(steps)); /* Enabling and Disabling GyroLock */ @@ -310,6 +337,9 @@ public void onAction() { }); */ + + + Control.autoClimb.setPressAction(new ButtonAction() { @Override public void onAction() { @@ -364,10 +394,14 @@ 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()); 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); diff --git a/src/main/java/frc/team555/robot/utils/LimitedMotor.java b/src/main/java/frc/team555/robot/utils/LimitedMotor.java new file mode 100644 index 0000000..658a408 --- /dev/null +++ b/src/main/java/frc/team555/robot/utils/LimitedMotor.java @@ -0,0 +1,31 @@ +package frc.team555.robot.utils; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.SpeedController; +import org.montclairrobotics.sprocket.motors.Motor; +import org.montclairrobotics.sprocket.utils.Input; + +public class LimitedMotor extends Motor { + private final DigitalInput upperBound; + private final DigitalInput lowerBound; + + public LimitedMotor(SpeedController motor, boolean brakeMode, DigitalInput lowerBound, DigitalInput upperBound) { + super(motor, brakeMode); + this.lowerBound=lowerBound; + this.upperBound=upperBound; + } + + @Override + public void set(double power) + { + if(power>0 && 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; + } +} From 075b4e12fcedfa5c926ee7fdd41ff992c53b1aba Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 13:02:54 -0400 Subject: [PATCH 26/38] Deprecated unused files --- src/main/java/frc/team555/robot/auto/AutoClimbSequence.java | 2 +- src/main/java/frc/team555/robot/auto/AutoStart.java | 2 +- src/main/java/frc/team555/robot/auto/SwitchAuto2.java | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java index d92bcd9..add250f 100644 --- a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java +++ b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java @@ -12,7 +12,7 @@ import org.montclairrobotics.sprocket.states.StateMachine; import static frc.team555.robot.components.MainLift.TOP; - +@Deprecated public class AutoClimbSequence extends StateMachine { private Lift lift; diff --git a/src/main/java/frc/team555/robot/auto/AutoStart.java b/src/main/java/frc/team555/robot/auto/AutoStart.java index 23865fe..7343de4 100644 --- a/src/main/java/frc/team555/robot/auto/AutoStart.java +++ b/src/main/java/frc/team555/robot/auto/AutoStart.java @@ -3,7 +3,7 @@ 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/SwitchAuto2.java b/src/main/java/frc/team555/robot/auto/SwitchAuto2.java index 28b382e..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; From 3a4cf1400a0717cb2136915e7c65b97f89e5b36c Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 13:22:27 -0400 Subject: [PATCH 27/38] Don't want to do this --- libs/Sprocket.jar | Bin 108275 -> 109816 bytes src/main/java/frc/team555/robot/Robot.java | 6 ++ .../frc/team555/robot/auto/AutoSwitch.java | 19 ++++ .../frc/team555/robot/auto/AutoSwitchLR.java | 12 +++ .../frc/team555/robot/auto/AutoSwitchMid.java | 30 ++++++ .../frc/team555/robot/auto/DropCubeScale.java | 10 ++ .../team555/robot/auto/DropCubeSwitch.java | 28 ++++++ .../team555/robot/auto/IntakeLiftMove.java | 40 ++++++++ .../auto/{DropCube.java => OldDropCube.java} | 7 ++ .../frc/team555/robot/auto/OldSideAuto.java | 18 ++-- .../team555/robot/auto/SetIntakeRotation.java | 4 + .../frc/team555/robot/auto/SwitchAuto.java | 24 ++++- .../team555/robot/components/CubeIntake.java | 94 ++++++++++++++++++ .../team555/robot/components/IntakeLift.java | 19 ++++ .../team555/robot/components/MainLift.java | 55 ++++++++++ .../java/frc/team555/robot/core/Control.java | 32 ++++++ .../java/frc/team555/robot/core/Hardware.java | 16 +++ .../frc/team555/robot/core/PowerUpRobot.java | 67 ++++++++++++- .../team555/robot/test/EncoderTestRobot.java | 36 +++++++ .../frc/team555/robot/utils/TargetMotor.java | 4 + 20 files changed, 506 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitch.java create mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitchLR.java create mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitchMid.java create mode 100644 src/main/java/frc/team555/robot/auto/DropCubeScale.java create mode 100644 src/main/java/frc/team555/robot/auto/DropCubeSwitch.java create mode 100644 src/main/java/frc/team555/robot/auto/IntakeLiftMove.java rename src/main/java/frc/team555/robot/auto/{DropCube.java => OldDropCube.java} (83%) create mode 100644 src/main/java/frc/team555/robot/test/EncoderTestRobot.java diff --git a/libs/Sprocket.jar b/libs/Sprocket.jar index 296441538f74ec09fea95088f85bdf5389e41299..7b25d528ce0c429653c9c5c2c5030e37dee12c80 100644 GIT binary patch delta 5169 zcmZu#c_39=7eD)6u1ojw%n34TeZ0a)}IArY=sV2E{8$!!0*2 zD(Y#D(x8_@qh5Rs9#7@z+h_0d>C^Z5qw}-YZ>{}XYwvT`*1#Rs@Dmo--GwP8MG$N@ z(Xl=_HU(wO2lmpGAU)iagbixqs2QbUA0!Ybd)q|JIU4IFNm0eNfnt@V!Ff1WooHSVt;MQaUHu;nfIKnp>TX52qS-RpVI3@2eKtO+QI^Gul zW-GxOAxrR~h=UxXMQxaRuk09N2zMT8!ddK&9Kou4kJ%!PLVe+UCW;EJn~OnBeF6rC zjV34$JdsX_a>`oSZ@;w5}5TpLI$aRLly-D)&us^lwg&Y5&VVX1o4c64$H@VK!Rt#Eh=h#c=VEp z@Qu+CQO4^+1GjE%^j@`9VWGn4jtHR&i|x!N*;Dnj<@Y9w*=e%u8FC!$Gn(@oF8B4F zDh`SbE-vRfG*rA0y8W4TvSHVHr2>Onx`##-M`eybI2kfCS$p%e<6b)Fe!bv!U|{gw z*3!}b)&q&GE$lVb9hvRi4qkH4)lXfrqcyQ&NZI|-$FROKVf!i<&T%cRuA z+ogiVUc{MvOh{&G6_GP*E{n383qmg3U82=_dd|#`jb(wwwULpb!#B?t5AZr4d~vxZ zrttJ!!ya{m_{5^m;Q_B*ul?P4R@vJ7o}N-dYsu*pspa>a%4;^b&-&N--JVlU@hl&D zcE94coGPgDwZD1ew_x|rVads}mw#JSb!TP!_2TeH);)2vo<`>mx-=wh?TR*fk*WFo zUE14L>+quE&dJ-vVmEx5W#0NvRfwr4FWF6>q5V~j|JMY+h&A>8C*9tET6isWerVov zg%0Z%=ZES&ZNt9344$oA^5@Fn@aDhN>uYDMYbsc#VBOFXz%5GXib)YFDu1jtn;C8Y z-m|a1nK4@a{W0^Ly}`2zrT*?$3&Mx$9+#HME%<0VDX`^0m1E6Lr+;ZL-n~aNu8bElUwy`n2XL(f8p(ieRtzlH=UnyBH!_Q z?Bjcp`X^tl8q#-66UJ?1M7t<*Svi|KQj{&D=Ra=C{D;xjwmY0_5+F(=Ppz<9YCpM_ z;bspWTb2=TTbu zp6eHXH?vb=N}RX7{&22??8}bB^OD+rpEGsGgo=TT_u7(;&c%+NUGs8TjhiP|Ki4O_ z);+V|@b@vrg)+Qj(uHRWt+NxJTjpD>Gxu#vGdde>*#F0(;{G`&xs8Vswolc_FiCf7 zo~%~2ernkbn}yBA4NDXpr`XgUjr!xpvCa>QpI86x;Te~e7-#N3A*Zrk_DXI6$<}4qNQJ0#X(O05so?ZQ8K*r~O7fL5 zRWnMgvzDExSGuZYu_iG|DSX*JzI*ZU)hJA=>FiZQy1n@v%_KGj2mlSXx6Ps zm^9G1$6{hu*q$49&qp{P63clWBmDY7uJFBV<>Yp^#;i~Ao`VnGk8E4elaB%X)@ex_;c4sE=^w_iej};nWBaBZ#?#FnhobPvm=p2k=O@;{;7e zf?sJqhdd<<=OO$|%M0s<`S>2wt($nD8HQ^x>1kOEuN3K{x6uvcoHr1nY$6U5)S_W0 zYGi{-G_|Rrw2&Ibq7z*37hbFLqYv*iK@d^^ul0-zB+31yK#F|(3%EcH4^gEokYdwg z#FUgN2NL8v6?l=Oe8Vc>4w_G*>uP{xpdq7ab9+ZRoQR2%WS}FI4;AS<60#$(+Bf#(ibdV!5LWf6egNn6q{Ug04Rzs8iEEi z4@EnTfGpOMZvvVWkUlR5Fp6#7j^GXE&2t7;Xzq%(yMl8_l*rT_%)xz&@C3i1R*I%C z2a>36k+mOqhhw!@gM%!j{#p>w!!5M(_VSQ}*Ox9as$N4fXCq*Vj6;AfPUy1{WHQ*) z4eo8~mPFEjBd`}u4+VJBMO(tbYs4dcB0(3@JUa^DtC`3n8VCUs-m*!F7+^s;N!u7O zk6NEhiUB4FoQVNw{F6g5U@@}T#{xG53S&V40>iPu9|523U^N16c|etXupQW--{j&z zAObOQU>yRTaUdLlB|AVU0u4K8i%~oc_Q%sMZUWedIB^MJJpw%mU@ZcRcG8yGonQm9 zs3d|QR7z4J@I;n}iL`4zk5+ex2RT%g>?9D4051uIAn+~;1S8;?3^pNfDH&`*V8$+5 z*x_9?hm%6D9-Trf>`tLw&Z!Vk9X<0Opt_s3r0%8_2JDATVx<8~-1;<_jEbnD0Hsbk zoo-uHI&3uMdYBH^p@3Zm)JuWm8FVRfne-k+Wx^^cS9>NMWyJ@6C}AO=b`A02K2RLL zELa=`TC->i+Bx?~K9NIVc1gnrst_U7vYQdl)x_4KL=!DZq*fPqMN&*jb8UJy& zC5YMN8xqXqj=S2DhXg==3Jm$_0}MZY^v{uG=`Z9V66lfY`+(LI>XohVkKJew-|as5 zt+x9Q4?d412Y$oUjD0{CQGa~331Y@LDsvQ31=YhG(y0`15dFtzoFL4{(c77%>_MPO zx)*~fh}x#pe$5|>AmB`}8Ap91K1St|7mI-^+K^5N7-j4+7l+g-0dmM?19y(GL>jxO z>pz)P0wy6BwW*9jIqZUCK+4bsFcg^hFMX;P3H;@(!%&6`iy*AV$(SoY7Ntzy$fqm( zF+JK9Cg;Y>q8b*RKMbe$_{$Gu!&a1eU)bT&z>`R41 zb@o`b)FEs_5f-G_0f6Qmsd@nBVVY5SEXYu3Mb)I-?bi{)1b#4q{kQ~Nb4NkZ4 z-2lG%w&RrdhLf_DU>13BBaj%kanzeiT{QlGSac*6=86txKqVALL{kQ9@?0e?_ajSHVvT^_86t0pn&Ywr;6{-J+~~%Gwi$t#_eSnn&=6C1wOsNd316@o#+w!+ih% delta 3672 zcmZ8jdr*|u75@$k3+OI_OF#ugM35DE2!A!0%}UD)aL8#)euLapXKPNAz7U6XW{ss(`Di4=wcF} z#Bfc3rEUiW=o(dOOI{|g7&|9CH(}~@pO`8eMOmv=vs3*cZezB#h?uNxsRIl)dBB|bp;{qI=9wodVMS6;Ozsyu^=;Z1r{l#nW)RJTCyX{GHV9O|> zY1q2hkz4*$kV8kRrd@qps^%AVlQMgEmyJhYug=GXg4+0`QULp>-XddTXEbHJzViw>sZ*ZcCBv|;KrIJsfj5MzQGNF6 z#+RaN9zBU-F*fv`6u`Of1~tplt^sGUU*GxPxthWYvcpyOW6rjqw}fip6LPRLI>6_` zW4RO5+tNsR=~f+J?bYDOA`Rj?u*4bQf8@=g-Jnl2RmmK6$zDvU)r)f%+%FeX5y#9{?}-zFBS>;k;NCpVUwmH3hWnaxk9H zL);98*}~Cz3W&A0j0p!9VGDj6uDDWC5k)7w%~;48T+L7h^TB8_#=%t~dzk<~(gZC3 zcpoBYf|j=y!e1y4%Z*@YuLxC^1!*Gus|~PMpn~iHr2iN?M1Nm> z0*|TWds{&~@s@vY2Q%f(>T+Nzzs%Xg9GK1Tapsi^Ap{oW0xc6$b0LoRleOo<0s>)q z@CO2wd5}WjRUWJ$@VpW{Shfiw>A$lkNF`v%hcp7)^I-#lXZestVA&37X&V5M1(Iqi zfK1Z3?S$0?Ogmu}fu{qO_+9W3S-#i>>9pcrg|Lt;n+qlNs8D7%Yd30mDfPSIV*;Lg zU=4vidmw{A_a0bFAh`%O5x7+(GmI*hn#N+8kIP<}VcuS(_yhLi09a5WE$0V-&pw%9 z@jgf-@xy%>hkMQ0F9SC3mmM2%0DH`-%?I!Z4t_X*sd5loDyukKD%(Gz3=8DcrZVYu zrwo!Q`LJ?HspU8VuIVYqx;V(HkQN$RR|O7@n*$HZ#19>m`Su=!jYO?iVLe5Cq+*k} zIPQ>4>d%L;+1%n?DJ^>|WiA+g2~l&aWWbgxtdfhJ8P1b~Eet-O$k&+6ck*GmWMzk? z_~l_qEjl7C=Z{E@$5B{Gaq^DJ)BiaDl8(WLMB(~+u(o5k@cckHE_;`HTt;m@jxFPq z&k0B(@aP13l%Ie}Z2YI-I}R5o;Z(}j%sF@;JhxSpXu99UpSpstU#T@wK6tn4)#l!} zSi`5FBadiZ`tTiidz9$#TC?)Vcq2UwL+bX4rjxDsynExDCEQ9K(Z|*N?sa9ECqd`K z(;2#$6(!P2K2HHPHF+G})eP_I5p)K%_{mA|(Dd63@;Vk+4I_z)sz&OZ15x}n!FE@} z7&?guAH2{9PjXheqL=D{CAoo-{ZuWp$}L%>twnLXR#74bwTe<#P(7fXT?1q2#N--0 zkvUU{aQ0b^xS!dV+xOv`t(rlcM?Vn#H!}**+E^MCUR-XYk zaxro9x&&#CUkL8(!5O(nPgZ!2?8cOX@L{ulbbJ$~!{i1SP5ZfITfqXGV-xnMBSV_6Wx;S} zYQna1?f~cFGl`qVW^M#~_H2Ob;oQA-#LZ^gaj;bWx1Wc1l;RHRu>~`%N=n;6+IQH6 zW_cmpYUbgyWd{Sd;yto9UruyN3vxl5C8ytpY3*o{Tp4l@!wQ-qfZb^UcPEo#QaW%j KZ?@o&l>Y+|JTy4~ diff --git a/src/main/java/frc/team555/robot/Robot.java b/src/main/java/frc/team555/robot/Robot.java index c8785c1..7c548cb 100644 --- a/src/main/java/frc/team555/robot/Robot.java +++ b/src/main/java/frc/team555/robot/Robot.java @@ -1,9 +1,15 @@ package frc.team555.robot; +<<<<<<< HEAD import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.test.TestRobot; import frc.team555.robot.test.TestRobot2; public class Robot extends PowerUpRobot { +======= +import frc.team555.robot.test.EncoderTestRobot; + +public class Robot extends EncoderTestRobot { +>>>>>>> josh-auto-switch } 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..44325b4 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitch.java @@ -0,0 +1,19 @@ +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 { + 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]); + } +} 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..2a23ac5 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -0,0 +1,12 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.utils.Side; +import org.montclairrobotics.sprocket.states.State; + +class AutoSwitchLR extends AutoSwitch { + AutoSwitchLR(Side pos, Side target) { + super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")" + + ); + } +} 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..edb595c --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -0,0 +1,30 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.components.IntakeLift; +import frc.team555.robot.core.Hardware; +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.DriveEncoderLock; +import org.montclairrobotics.sprocket.auto.states.ResetGyro; +import org.montclairrobotics.sprocket.geometry.Angle; + +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, 0.75, Angle.ZERO, true, PowerUpRobot.correction), + /* Turn π/2 radians (in the correct directon) */ + new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * 0.75, Angle.QUARTER, true, PowerUpRobot.correction), + /* Drive forward 36 inches */ + new DriveEncoderGyro(36, 0.75, Angle.ZERO, true, PowerUpRobot.correction), + /* Move intake lift up 8 inches */ + new IntakeLiftMove(8, 0.75, true), + /* Turn π/2 radians (in the opposite direction) */ + new DropCubeSwitch(target, PowerUpRobot.correction) + ); + } +} diff --git a/src/main/java/frc/team555/robot/auto/DropCubeScale.java b/src/main/java/frc/team555/robot/auto/DropCubeScale.java new file mode 100644 index 0000000..ae76c9f --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/DropCubeScale.java @@ -0,0 +1,10 @@ +package frc.team555.robot.auto; + +import org.montclairrobotics.sprocket.states.MultiState; +import org.montclairrobotics.sprocket.states.StateMachine; + +public class DropCubeScale extends StateMachine { + public DropCubeScale() { + super(new MultiState()); + } +} diff --git a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java new file mode 100644 index 0000000..d4c53dc --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java @@ -0,0 +1,28 @@ +package frc.team555.robot.auto; + +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; +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.states.MultiState; +import org.montclairrobotics.sprocket.states.State; +import org.montclairrobotics.sprocket.states.StateMachine; + +public class DropCubeSwitch extends StateMachine { + + private CubeIntake intake; + private IntakeLift lift; + + public DropCubeSwitch(Side 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(PowerUpRobot.intake, 1)); + } +} diff --git a/src/main/java/frc/team555/robot/auto/IntakeLiftMove.java b/src/main/java/frc/team555/robot/auto/IntakeLiftMove.java new file mode 100644 index 0000000..bfdceeb --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/IntakeLiftMove.java @@ -0,0 +1,40 @@ +package frc.team555.robot.auto; + +import frc.team555.robot.core.Hardware; +import org.montclairrobotics.sprocket.states.State; + +public class IntakeLiftMove implements State{ + private double height; + private double power; + private boolean up; + + public IntakeLiftMove(double height, double power,boolean up) + { + this.height=height; + this.power=power; + this.up=up; + } + + + @Override + public void start() { + + } + + @Override + public void stop() + { + Hardware.motorLiftIntake.set(0); + } + + @Override + public void stateUpdate() { + Hardware.motorLiftIntake.set(power); + + } + + @Override + public boolean isDone() { + return Hardware.intakeLiftEncoder.getInches().get()>height==up; + } +} diff --git a/src/main/java/frc/team555/robot/auto/DropCube.java b/src/main/java/frc/team555/robot/auto/OldDropCube.java similarity index 83% rename from src/main/java/frc/team555/robot/auto/DropCube.java rename to src/main/java/frc/team555/robot/auto/OldDropCube.java index 95beccd..e495c47 100644 --- a/src/main/java/frc/team555/robot/auto/DropCube.java +++ b/src/main/java/frc/team555/robot/auto/OldDropCube.java @@ -13,8 +13,15 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; +<<<<<<< HEAD:src/main/java/frc/team555/robot/auto/DropCube.java public class DropCube extends StateMachine{ public DropCube(MainLift mainLift,CubeIntake intake, GyroCorrection correction, Input side){ +======= + +@Deprecated +public class OldDropCube extends StateMachine{ + public OldDropCube(CubeIntake intake, MainLift lift, GyroCorrection correction, Side side){ +>>>>>>> josh-auto-switch:src/main/java/frc/team555/robot/auto/OldDropCube.java super( new MultiState( new MoveLift(mainLift,MainLift.TOP*0.6,1,true), diff --git a/src/main/java/frc/team555/robot/auto/OldSideAuto.java b/src/main/java/frc/team555/robot/auto/OldSideAuto.java index a8dd592..1550c71 100644 --- a/src/main/java/frc/team555/robot/auto/OldSideAuto.java +++ b/src/main/java/frc/team555/robot/auto/OldSideAuto.java @@ -1,12 +1,11 @@ package frc.team555.robot.auto; -import edu.wpi.first.wpilibj.interfaces.Gyro; -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.core.PowerUpRobot; import frc.team555.robot.utils.Side; -import frc.team555.robot.core.Hardware; import org.montclairrobotics.sprocket.auto.states.*; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; @@ -15,14 +14,13 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.PID; import java.util.ArrayList; -@Deprecated public class OldSideAuto implements State { CubeIntake intake; IntakeLift intakeLift; + MainLift mainLift; StateMachine machine; ArrayList states = new ArrayList(); Input crossover; @@ -30,9 +28,10 @@ public class OldSideAuto implements State { GyroCorrection correction; - public OldSideAuto(CubeIntake intake, IntakeLift intakeLift, GyroCorrection gyroCorrection){ + public OldSideAuto(CubeIntake intake, IntakeLift intakeLift, MainLift mainLift, GyroCorrection gyroCorrection){ this.intake = intake; this.intakeLift = intakeLift; + this.mainLift = mainLift; correction = gyroCorrection; @@ -62,13 +61,10 @@ public void start() { states.add(new ResetGyro(correction)); states.add(new SetIntakeRotation(intake, intake.downPos)); - if(Side.fromDriverStation()[0] == Side.RIGHT){ + if(Side.fromDriverStation()[0] == PowerUpRobot.startSidesChooser.getSelected()){ states.add(new DriveEncoderGyro(168, .75, Angle.ZERO, false, correction)); - states.add(new TurnGyro(new Degrees(90), correction, false)); + states.add(new DropCubeSwitch(Side.fromDriverStation()[0], correction)); Debug.msg("Target", "RightSwitch"); - //states.add(new Enable(intake)); - //states.add(new Delay(1)); - //states.add(new Disable(intake)); }else if(crossover.get()){ states.add(new DriveEncoderGyro(100, .75, Angle.ZERO, false, correction)); states.add(new DriveEncoderGyro(150, .75, new Degrees(90), false, correction)); diff --git a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java index 6c5b03a..95a0282 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java @@ -15,7 +15,11 @@ public SetIntakeRotation(CubeIntake intake, int position) { @Override public void start() { +<<<<<<< HEAD // intake.roationalMotor.setTarget(position); +======= + intake.rotationalMotor.setTarget(position); +>>>>>>> josh-auto-switch } @Override diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index e4a5e15..48c05ae 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -3,8 +3,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.components.CubeIntake; +<<<<<<< HEAD import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; +======= +import frc.team555.robot.components.MainLift; +import frc.team555.robot.components.SimpleIntake; +>>>>>>> josh-auto-switch import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.auto.states.Delay; @@ -15,26 +20,29 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; - +@Deprecated public class SwitchAuto extends StateMachine{ static Input startSide; - public static SendableChooser startSidesChooser; public static void init(){ +<<<<<<< HEAD startSidesChooser = new SendableChooser<>(); for(Side side : Side.values()){ startSidesChooser.addObject(side.toString(), side); } //SmartDashboard.putData(startSidesChooser); +======= +>>>>>>> josh-auto-switch startSide = new Input() { @Override public Boolean get() { - return Side.fromDriverStation()[0] == startSidesChooser.getSelected(); + return Side.fromDriverStation()[0] == PowerUpRobot.startSidesChooser.getSelected(); } }; } +<<<<<<< HEAD public static void disabled(){ SmartDashboard.putData("Start Position", startSidesChooser); } @@ -57,5 +65,15 @@ public Boolean get() { return !SwitchAuto.startSide.get(); } })); +======= + public static Boolean getStartSide(){ + return startSide.get(); + } + + public SwitchAuto(GyroCorrection correction, CubeIntake intake, MainLift lift){ + super(new ResetGyro(correction), + new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)); + // new ConditionalState(new DropCube(intake, lift, PowerUpRobot.startSidesChooser.getSelected(), startSide)); +>>>>>>> josh-auto-switch } } diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index bcbc962..6e779d2 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -12,6 +12,7 @@ import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; @@ -21,6 +22,7 @@ public class CubeIntake implements Updatable, Togglable{ +<<<<<<< HEAD private double rotatePower=.1; private boolean auto; @@ -29,12 +31,20 @@ public class CubeIntake implements Updatable, Togglable{ public Motor right; public TargetMotor rotate; public final double tolerance = 1; // Todo: Maybe needs to be tuned +======= + 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 +>>>>>>> josh-auto-switch public final int upPos = 0; public final int downPos = 2; public final int middlePos = 1; +<<<<<<< HEAD public Input power; public CubeIntake() { @@ -47,6 +57,23 @@ public CubeIntake() { this.rotate=new TargetMotor(Hardware.intakeRotationEncoder,new BangBang(tolerance,rotatePower),rotateMotor); // this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented +======= + public final 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 + +>>>>>>> josh-auto-switch this.power = new Input() { @Override public Vector get() { @@ -63,6 +90,7 @@ public Vector get() { return new XY(x,y); } }; +<<<<<<< HEAD Control.intakeRotateDown.setPressAction(new ButtonAction() { @@ -77,16 +105,39 @@ public void onAction() { @Override public void onAction() { rotate.setTarget(upPos); +======= + Control.intakeRotationDown.setPressAction(new ButtonAction() { + @Override + public void onAction() { + rotationalMotor.set(downPos); + } + }); + Control.intakeRotationUp.setPressAction(new ButtonAction() { + @Override + public void onAction() { + rotationalMotor.set(upPos); +>>>>>>> josh-auto-switch } }); Control.intakeRotateMiddle.setPressAction(new ButtonAction() { @Override public void onAction() { +<<<<<<< HEAD rotate.setTarget(middlePos); +======= + rotationalMotor.set(middlePos); +>>>>>>> josh-auto-switch } }); + Control.intakeLiftManualUp.setPressAction(new ButtonAction() { + @Override + public void onAction() { + rotationalMotor.set(rotationalMotorPower); + } + }); +<<<<<<< HEAD Control.intakeRotateUpManual.setPressAction(new ButtonAction() { @Override public void onAction() { @@ -114,6 +165,37 @@ public void onAction() { }); rotate.setPower(0); +======= + Control.intakeLiftManualUp.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + rotationalMotor.set(rotationalMotorPower); + } + }); + + + new CurrentMonitor("Intake Right Motor", Hardware.motorIntakeR, new Input() { + @Override + public Boolean get() { + return power.get().getMagnitude() > .1; + } + }); + + new CurrentMonitor("Intake Left Motor", Hardware.motorIntakeL, new Input() { + @Override + public Boolean get() { + return power.get().getMagnitude() > .1; + } + }); + + new CurrentMonitor("Intake Rotational Motor", Hardware.motorRotational, new Input() { + @Override + public Boolean get() { + return Math.abs(rotationalMotor.getTarget() - rotationalMotor.getDistance().get()) > 20; + } + }).setEncoder(Hardware.intakeRotationEncoder); + +>>>>>>> josh-auto-switch Updater.add(this, Priority.CALC); } @@ -121,6 +203,7 @@ public void onAction() { @Override public void update() { Vector p = power.get(); +<<<<<<< HEAD if(!auto) { left.set(p.getY() - p.getX()*.25); right.set(p.getY() + p.getX()*.25); @@ -141,6 +224,17 @@ public void setAutoPower(double power){ auto = true; left.set(power); right.set(power); +======= + // if(p.getMagnitude() < tolerance){ + // openClamp(); + // }else{ + // closeClamp(); + // } + left.set(p.getX()); + right.set(p.getY()); + + // updateClamp(); +>>>>>>> josh-auto-switch } @Override diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 9b744be..20a2826 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -5,8 +5,10 @@ import frc.team555.robot.utils.BangBang; import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; +import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; +import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.Utils; @@ -30,7 +32,11 @@ public class IntakeLift implements Lift{ * Constructor for IntakeLift Class with default position of 0 */ public IntakeLift() { +<<<<<<< HEAD motors = new TargetMotor(Hardware.intakeLiftEncoder, new BangBang(1,1),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng +======= + motors = new TargetMotor(Hardware.liftEncoder, new PID(0, 0, 0),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng +>>>>>>> josh-auto-switch encoder=Hardware.intakeLiftEncoder; //================= @@ -95,9 +101,22 @@ public void onAction() { motors.setPower(0); } }; +<<<<<<< HEAD Control.intakeLiftManualUp.setReleaseAction(stop); Control.intakeLiftManualDown.setReleaseAction(stop); motors.setPower(0); +======= + Control.intakeLiftManualUp.setOffAction(stop); + Control.intakeLiftManualDown.setOffAction(stop); + + new CurrentMonitor("Intake Lift Encoder", Hardware.motorLiftIntake, new Input() { + @Override + public Boolean get() { + return Control.intakeLiftManualUp.get() || Control.intakeLiftManualUp.get() || Math.abs(motors.getTarget() - motors.getDistance().get()) > 20; + } + }).setEncoder(Hardware.intakeLiftEncoder); + +>>>>>>> josh-auto-switch } diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 3c65752..94812e4 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -1,20 +1,33 @@ package frc.team555.robot.components; +<<<<<<< HEAD import frc.team555.robot.auto.MoveLift; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.BangBang; +======= +import edu.wpi.first.wpilibj.DigitalInput; +import frc.team555.robot.core.Control; +import frc.team555.robot.core.Hardware; +import frc.team555.robot.utils.MotorMonitor; +>>>>>>> josh-auto-switch 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; +<<<<<<< HEAD import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.states.State; import org.montclairrobotics.sprocket.states.StateMachine; +======= +import org.montclairrobotics.sprocket.motors.CurrentMonitor; +import org.montclairrobotics.sprocket.motors.Motor; +import org.montclairrobotics.sprocket.utils.Input; +>>>>>>> josh-auto-switch import org.montclairrobotics.sprocket.utils.PID; import java.nio.channels.Pipe; @@ -29,9 +42,14 @@ public class MainLift extends TargetMotor implements Lift { private int upPosition; private int downPosition; + private DigitalInput bottomLimitSwitch; public MainLift(){ +<<<<<<< HEAD super(Hardware.liftEncoder, new BangBang(1,1), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); +======= + super(Hardware.liftEncoder, new PID(.1, 0, 0), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); +>>>>>>> josh-auto-switch mode = Mode.POWER; @@ -54,6 +72,7 @@ public void onAction() { Control.mainLiftManualDown.setHeldAction(new ButtonAction() { @Override public void onAction() { +<<<<<<< HEAD if(LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) { if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); @@ -64,14 +83,29 @@ public void onAction() { }else{ set(0); encoder.reset(); +======= + if(!bottomLimitSwitch.get()) { + MainLift.super.set(-speed); + }else{ + MainLift.super.set(0); + Hardware.liftEncoder.reset(); +>>>>>>> josh-auto-switch } } }); + + Control.mainLiftManualDown.setReleaseAction(new ButtonAction() { @Override public void onAction() { +<<<<<<< HEAD set(0); +======= + if(encoder != null) { + MainLift.super.set(0); + } +>>>>>>> josh-auto-switch } }); @@ -99,6 +133,7 @@ public void onAction() { } } }); +<<<<<<< HEAD Control.mainLiftAutoUp.setReleaseAction(new ButtonAction() { @Override public void onAction() { @@ -107,6 +142,26 @@ public void onAction() { } }); setPower(0); +======= + + + + new CurrentMonitor("Main Lift Front", Hardware.motorLiftMainFront, new Input() { + @Override + public Boolean get() { + return Math.abs(getTarget() - getDistance().get()) > 20 || speed != 0; + } + }).setEncoder(Hardware.liftEncoder); + + 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); + + +>>>>>>> josh-auto-switch // Lift Bottom diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 28b1c3b..8fb8df5 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -29,6 +29,7 @@ public class Control { mainLiftAutoUp, mainLiftManualUp, mainLiftManualDown, +<<<<<<< HEAD // mainLiftTop, // mainLiftBottom, //openButton, @@ -44,6 +45,22 @@ public class Control { //public static int FieldCentricID=2; //public static int ResetID=3; +======= + mainLiftTop, + mainLiftBottom, + openButton, + closeButton, + intakeRotationUp, + intakeRotationDown, + intakeRotationMiddle, + intakeSubroutine, + intakeRotationManualUp, + intakeRotationManualDown; + + + public static int FieldCentricID=2; + public static int ResetID=3; +>>>>>>> josh-auto-switch // Joystick Declaration public static Joystick driveStick; public static Joystick auxStick; @@ -72,6 +89,7 @@ public static void init() { // intakeLiftIncrement = new JoystickButton(auxStick,12); // intakeLiftDecrement = new JoystickButton(auxStick,12); +<<<<<<< HEAD // mainLiftTop = new JoystickButton(auxStick,9); // mainLiftBottom = new JoystickButton(auxStick,8); @@ -98,6 +116,20 @@ public static void init() { // intakeLiftBottom = new JoystickButton(auxStick, 13); // intakeSubroutine = new JoystickButton(auxStick, 14); +======= + // mainLiftTop = new JoystickButton(auxStick,12); + // mainLiftBottom = new JoystickButton(auxStick,12); + mainLiftManualUp = new JoystickButton(auxStick,11); + mainLiftManualDown = new JoystickButton(auxStick,10); + intakeLiftManualUp = new JoystickButton(auxStick,3); + intakeLiftManualDown = new JoystickButton(auxStick,2); + intakeLiftTop = new JoystickButton(auxStick, 6); + intakeLiftBottom = new JoystickButton(auxStick, 7); + intakeSubroutine = new JoystickButton(auxStick, 14); + intakeRotationUp = new JoystickButton(auxStick,8); + intakeRotationManualUp = new JoystickButton(auxStick, 9); + intakeRotationManualDown = new JoystickButton(auxStick, 8); +>>>>>>> josh-auto-switch // openButton=new JoystickButton(Control.auxStick,8); // closeButton=new JoystickButton(Control.auxStick,9); diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 83d72f4..3778714 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -63,6 +63,13 @@ private static class DeviceID { public static final int motorIntakeL = 10; public static final int motorIntakeR = 5; +<<<<<<< HEAD +======= + public static final int motorRotational = 10; + + public static final int motorIntakeL = 5; + public static final int motorIntakeR = 10; +>>>>>>> josh-auto-switch //left intake 5 //pincer 6 //lifter 9 @@ -140,6 +147,9 @@ public static void init(){ motorIntakeR = new WPI_TalonSRX(DeviceID.motorIntakeR); motorIntakeL = new WPI_TalonSRX(DeviceID.motorIntakeL); + motorIntakeL = new WPI_TalonSRX(DeviceID.motorIntakeL); + motorIntakeR = new WPI_TalonSRX(DeviceID.motorIntakeR); + motorLiftMainBack.setInverted(true); //motorIntakeClamp = new WPI_TalonSRX(DeviceID.motorIntakeClamp); @@ -172,9 +182,15 @@ public static void init(){ //Intake Lift Travel //15.5 to 49.5 +<<<<<<< HEAD liftEncoder = new SEncoder(new Encoder(4,5), 1/*31600/(82 - 54)*/); // todo: REALLY NEED TO BE SET intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1 /*1333600/(49.5 - 15.5)*/); intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // todo, needs to be set +======= + liftEncoder = new SEncoder(new Encoder(4,5), 31600/(82 - 54)); // todo: REALLY NEED TO BE SET + intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1333600/(49.5 - 15.5)); + intakeRotationEncoder = new TalonEncoder(motorRotational, 1); // todo, needs to be set +>>>>>>> josh-auto-switch navx = new NavXInput(DeviceID.navxPort); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 80d6cb0..b0db476 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -1,15 +1,22 @@ package frc.team555.robot.core; +<<<<<<< HEAD import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; +======= +>>>>>>> josh-auto-switch import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; +<<<<<<< HEAD import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.CoastMotor; +======= +import frc.team555.robot.utils.MotorMonitor; +>>>>>>> josh-auto-switch import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; @@ -21,6 +28,7 @@ import org.montclairrobotics.sprocket.drive.steps.Sensitivity; import org.montclairrobotics.sprocket.drive.utils.GyroLock; import org.montclairrobotics.sprocket.geometry.*; +import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Module; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.pipeline.Step; @@ -35,6 +43,7 @@ public class PowerUpRobot extends SprocketRobot { DriveTrain driveTrain; public static GyroCorrection correction; +<<<<<<< HEAD Sensitivity sensitivity; GyroLock lock; boolean manualLock; @@ -45,6 +54,13 @@ public class PowerUpRobot extends SprocketRobot { public static Side startSide; //SendableChooser startSideChooser; static Input switchOnSide; +======= + GyroLock lock; + boolean manualLock; + public static CubeIntake intake; + + public static SendableChooser startSidesChooser; +>>>>>>> josh-auto-switch //vision stuff @@ -104,6 +120,33 @@ public Boolean get() { new CoastMotor(Hardware.motorDriveBR,false), new CoastMotor(Hardware.motorDriveFR,false)); + new CurrentMonitor("Front Right Drive Motor", Hardware.motorDriveFR, new Input() { + @Override + public Boolean get() { + return Control.driveStick.getMagnitude() > .1; + } + }).setEncoder(Hardware.rightDriveEncoder); + + new CurrentMonitor("Back Right Drive Motor", Hardware.motorDriveBR, new Input() { + @Override + public Boolean get() { + return Control.driveStick.getMagnitude() > .1; + } + }).setEncoder(Hardware.rightDriveEncoder); + + new CurrentMonitor("Front Left Drive Motor", Hardware.motorDriveFL, new Input() { + @Override + public Boolean get() { + return Control.driveStick.getMagnitude() > .1; + } + }).setEncoder(Hardware.leftDriveEncoder); + + new CurrentMonitor("Back Left Drive Motor", Hardware.motorDriveBL, new Input() { + @Override + public Boolean get() { + return Control.driveStick.getMagnitude() > .1; + } + }).setEncoder(Hardware.leftDriveEncoder); DriveTrainBuilder dtBuilder = new DriveTrainBuilder(); @@ -226,6 +269,9 @@ public void onAction() { correction)); + + + AutoMode encoder = new AutoMode("encoder", new DriveEncoders(100,.25)); @@ -332,7 +378,7 @@ public void onAction() { public void onAction() { shootCube.stop(); intake.disable(); - intake.roationalMotor.set(intake.downPos); + intake.rotationalMotor.set(intake.downPos); } }); */ @@ -375,7 +421,17 @@ public void onAction() { } }); visionThread.start();*/ +<<<<<<< HEAD // intakeLift.setPower(0); +======= + + startSidesChooser = new SendableChooser<>(); + for(Side side : Side.values()){ + startSidesChooser.addObject(side.toString(), side); + } + SmartDashboard.putData(startSidesChooser); + +>>>>>>> josh-auto-switch } @Override @@ -400,6 +456,7 @@ public void update(){ SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); +<<<<<<< HEAD SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); SmartDashboard.putNumber("POV",Control.auxStick.getPOV()); debugCurrent("Main Lift Front",Hardware.motorLiftMainFront); @@ -407,6 +464,10 @@ public void update(){ debugCurrent("Intake Lift",Hardware.motorLiftIntake); SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); +======= + Debug.msg("Intake Rotation", Hardware.intakeRotationEncoder.get()); + +>>>>>>> josh-auto-switch gyroLocking(); //startSide = startSideChooser.getSelected(); } @@ -436,8 +497,12 @@ private void gyroLocking(){ @Override public void userDisabledPeriodic(){ +<<<<<<< HEAD //SmartDashboard.putData(startSideChooser); //startSide = startSideChooser.getSelected(); SwitchAuto.disabled(); +======= + SmartDashboard.putData(startSidesChooser) ; +>>>>>>> josh-auto-switch } } diff --git a/src/main/java/frc/team555/robot/test/EncoderTestRobot.java b/src/main/java/frc/team555/robot/test/EncoderTestRobot.java new file mode 100644 index 0000000..6dfcfa9 --- /dev/null +++ b/src/main/java/frc/team555/robot/test/EncoderTestRobot.java @@ -0,0 +1,36 @@ +package frc.team555.robot.test; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.Joystick; +import frc.team555.robot.utils.TalonEncoder; +import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.drive.ControlledMotor; +import org.montclairrobotics.sprocket.utils.Debug; +import org.montclairrobotics.sprocket.utils.Input; + +public class EncoderTestRobot extends SprocketRobot { + + TalonEncoder testEncoder; + WPI_TalonSRX testMotorController; + ControlledMotor testMotor; + Joystick testJoystick; + + @Override + public void robotInit() { + testMotorController = new WPI_TalonSRX(0); + testEncoder = new TalonEncoder(testMotorController, 1); + testJoystick = new Joystick(0); + testMotor = new ControlledMotor(testMotorController, new Input() { + @Override + public Double get() { + return testJoystick.getY(); + } + }); + } + @Override + public void update() { + Debug.msg("Motor Position",testEncoder.get()); + Debug.msg("Motor Speed",testEncoder.getSpeed()); + } + +} diff --git a/src/main/java/frc/team555/robot/utils/TargetMotor.java b/src/main/java/frc/team555/robot/utils/TargetMotor.java index 1ab2bfa..bfaf839 100644 --- a/src/main/java/frc/team555/robot/utils/TargetMotor.java +++ b/src/main/java/frc/team555/robot/utils/TargetMotor.java @@ -83,4 +83,8 @@ public void update() { super.set(pid.get()); } } + + public double getTarget(){ + return pid.getTarget(); + } } From daa58019b1303b6ca49906f9b4266a93754cfaba Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 13:22:32 -0400 Subject: [PATCH 28/38] Revert "Don't want to do this" This reverts commit 3a4cf1400a0717cb2136915e7c65b97f89e5b36c. --- libs/Sprocket.jar | Bin 109816 -> 108275 bytes src/main/java/frc/team555/robot/Robot.java | 6 -- .../frc/team555/robot/auto/AutoSwitch.java | 19 ---- .../frc/team555/robot/auto/AutoSwitchLR.java | 12 --- .../frc/team555/robot/auto/AutoSwitchMid.java | 30 ------ .../auto/{OldDropCube.java => DropCube.java} | 7 -- .../frc/team555/robot/auto/DropCubeScale.java | 10 -- .../team555/robot/auto/DropCubeSwitch.java | 28 ------ .../team555/robot/auto/IntakeLiftMove.java | 40 -------- .../frc/team555/robot/auto/OldSideAuto.java | 18 ++-- .../team555/robot/auto/SetIntakeRotation.java | 4 - .../frc/team555/robot/auto/SwitchAuto.java | 24 +---- .../team555/robot/components/CubeIntake.java | 94 ------------------ .../team555/robot/components/IntakeLift.java | 19 ---- .../team555/robot/components/MainLift.java | 55 ---------- .../java/frc/team555/robot/core/Control.java | 32 ------ .../java/frc/team555/robot/core/Hardware.java | 16 --- .../frc/team555/robot/core/PowerUpRobot.java | 67 +------------ .../team555/robot/test/EncoderTestRobot.java | 36 ------- .../frc/team555/robot/utils/TargetMotor.java | 4 - 20 files changed, 15 insertions(+), 506 deletions(-) delete mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitch.java delete mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitchLR.java delete mode 100644 src/main/java/frc/team555/robot/auto/AutoSwitchMid.java rename src/main/java/frc/team555/robot/auto/{OldDropCube.java => DropCube.java} (83%) delete mode 100644 src/main/java/frc/team555/robot/auto/DropCubeScale.java delete mode 100644 src/main/java/frc/team555/robot/auto/DropCubeSwitch.java delete mode 100644 src/main/java/frc/team555/robot/auto/IntakeLiftMove.java delete mode 100644 src/main/java/frc/team555/robot/test/EncoderTestRobot.java diff --git a/libs/Sprocket.jar b/libs/Sprocket.jar index 7b25d528ce0c429653c9c5c2c5030e37dee12c80..296441538f74ec09fea95088f85bdf5389e41299 100644 GIT binary patch delta 3672 zcmZ8jdr*|u75@$k3+OI_OF#ugM35DE2!A!0%}UD)aL8#)euLapXKPNAz7U6XW{ss(`Di4=wcF} z#Bfc3rEUiW=o(dOOI{|g7&|9CH(}~@pO`8eMOmv=vs3*cZezB#h?uNxsRIl)dBB|bp;{qI=9wodVMS6;Ozsyu^=;Z1r{l#nW)RJTCyX{GHV9O|> zY1q2hkz4*$kV8kRrd@qps^%AVlQMgEmyJhYug=GXg4+0`QULp>-XddTXEbHJzViw>sZ*ZcCBv|;KrIJsfj5MzQGNF6 z#+RaN9zBU-F*fv`6u`Of1~tplt^sGUU*GxPxthWYvcpyOW6rjqw}fip6LPRLI>6_` zW4RO5+tNsR=~f+J?bYDOA`Rj?u*4bQf8@=g-Jnl2RmmK6$zDvU)r)f%+%FeX5y#9{?}-zFBS>;k;NCpVUwmH3hWnaxk9H zL);98*}~Cz3W&A0j0p!9VGDj6uDDWC5k)7w%~;48T+L7h^TB8_#=%t~dzk<~(gZC3 zcpoBYf|j=y!e1y4%Z*@YuLxC^1!*Gus|~PMpn~iHr2iN?M1Nm> z0*|TWds{&~@s@vY2Q%f(>T+Nzzs%Xg9GK1Tapsi^Ap{oW0xc6$b0LoRleOo<0s>)q z@CO2wd5}WjRUWJ$@VpW{Shfiw>A$lkNF`v%hcp7)^I-#lXZestVA&37X&V5M1(Iqi zfK1Z3?S$0?Ogmu}fu{qO_+9W3S-#i>>9pcrg|Lt;n+qlNs8D7%Yd30mDfPSIV*;Lg zU=4vidmw{A_a0bFAh`%O5x7+(GmI*hn#N+8kIP<}VcuS(_yhLi09a5WE$0V-&pw%9 z@jgf-@xy%>hkMQ0F9SC3mmM2%0DH`-%?I!Z4t_X*sd5loDyukKD%(Gz3=8DcrZVYu zrwo!Q`LJ?HspU8VuIVYqx;V(HkQN$RR|O7@n*$HZ#19>m`Su=!jYO?iVLe5Cq+*k} zIPQ>4>d%L;+1%n?DJ^>|WiA+g2~l&aWWbgxtdfhJ8P1b~Eet-O$k&+6ck*GmWMzk? z_~l_qEjl7C=Z{E@$5B{Gaq^DJ)BiaDl8(WLMB(~+u(o5k@cckHE_;`HTt;m@jxFPq z&k0B(@aP13l%Ie}Z2YI-I}R5o;Z(}j%sF@;JhxSpXu99UpSpstU#T@wK6tn4)#l!} zSi`5FBadiZ`tTiidz9$#TC?)Vcq2UwL+bX4rjxDsynExDCEQ9K(Z|*N?sa9ECqd`K z(;2#$6(!P2K2HHPHF+G})eP_I5p)K%_{mA|(Dd63@;Vk+4I_z)sz&OZ15x}n!FE@} z7&?guAH2{9PjXheqL=D{CAoo-{ZuWp$}L%>twnLXR#74bwTe<#P(7fXT?1q2#N--0 zkvUU{aQ0b^xS!dV+xOv`t(rlcM?Vn#H!}**+E^MCUR-XYk zaxro9x&&#CUkL8(!5O(nPgZ!2?8cOX@L{ulbbJ$~!{i1SP5ZfITfqXGV-xnMBSV_6Wx;S} zYQna1?f~cFGl`qVW^M#~_H2Ob;oQA-#LZ^gaj;bWx1Wc1l;RHRu>~`%N=n;6+IQH6 zW_cmpYUbgyWd{Sd;yto9UruyN3vxl5C8ytpY3*o{Tp4l@!wQ-qfZb^UcPEo#QaW%j KZ?@o&l>Y+|JTy4~ delta 5169 zcmZu#c_39=7eD)6u1ojw%n34TeZ0a)}IArY=sV2E{8$!!0*2 zD(Y#D(x8_@qh5Rs9#7@z+h_0d>C^Z5qw}-YZ>{}XYwvT`*1#Rs@Dmo--GwP8MG$N@ z(Xl=_HU(wO2lmpGAU)iagbixqs2QbUA0!Ybd)q|JIU4IFNm0eNfnt@V!Ff1WooHSVt;MQaUHu;nfIKnp>TX52qS-RpVI3@2eKtO+QI^Gul zW-GxOAxrR~h=UxXMQxaRuk09N2zMT8!ddK&9Kou4kJ%!PLVe+UCW;EJn~OnBeF6rC zjV34$JdsX_a>`oSZ@;w5}5TpLI$aRLly-D)&us^lwg&Y5&VVX1o4c64$H@VK!Rt#Eh=h#c=VEp z@Qu+CQO4^+1GjE%^j@`9VWGn4jtHR&i|x!N*;Dnj<@Y9w*=e%u8FC!$Gn(@oF8B4F zDh`SbE-vRfG*rA0y8W4TvSHVHr2>Onx`##-M`eybI2kfCS$p%e<6b)Fe!bv!U|{gw z*3!}b)&q&GE$lVb9hvRi4qkH4)lXfrqcyQ&NZI|-$FROKVf!i<&T%cRuA z+ogiVUc{MvOh{&G6_GP*E{n383qmg3U82=_dd|#`jb(wwwULpb!#B?t5AZr4d~vxZ zrttJ!!ya{m_{5^m;Q_B*ul?P4R@vJ7o}N-dYsu*pspa>a%4;^b&-&N--JVlU@hl&D zcE94coGPgDwZD1ew_x|rVads}mw#JSb!TP!_2TeH);)2vo<`>mx-=wh?TR*fk*WFo zUE14L>+quE&dJ-vVmEx5W#0NvRfwr4FWF6>q5V~j|JMY+h&A>8C*9tET6isWerVov zg%0Z%=ZES&ZNt9344$oA^5@Fn@aDhN>uYDMYbsc#VBOFXz%5GXib)YFDu1jtn;C8Y z-m|a1nK4@a{W0^Ly}`2zrT*?$3&Mx$9+#HME%<0VDX`^0m1E6Lr+;ZL-n~aNu8bElUwy`n2XL(f8p(ieRtzlH=UnyBH!_Q z?Bjcp`X^tl8q#-66UJ?1M7t<*Svi|KQj{&D=Ra=C{D;xjwmY0_5+F(=Ppz<9YCpM_ z;bspWTb2=TTbu zp6eHXH?vb=N}RX7{&22??8}bB^OD+rpEGsGgo=TT_u7(;&c%+NUGs8TjhiP|Ki4O_ z);+V|@b@vrg)+Qj(uHRWt+NxJTjpD>Gxu#vGdde>*#F0(;{G`&xs8Vswolc_FiCf7 zo~%~2ernkbn}yBA4NDXpr`XgUjr!xpvCa>QpI86x;Te~e7-#N3A*Zrk_DXI6$<}4qNQJ0#X(O05so?ZQ8K*r~O7fL5 zRWnMgvzDExSGuZYu_iG|DSX*JzI*ZU)hJA=>FiZQy1n@v%_KGj2mlSXx6Ps zm^9G1$6{hu*q$49&qp{P63clWBmDY7uJFBV<>Yp^#;i~Ao`VnGk8E4elaB%X)@ex_;c4sE=^w_iej};nWBaBZ#?#FnhobPvm=p2k=O@;{;7e zf?sJqhdd<<=OO$|%M0s<`S>2wt($nD8HQ^x>1kOEuN3K{x6uvcoHr1nY$6U5)S_W0 zYGi{-G_|Rrw2&Ibq7z*37hbFLqYv*iK@d^^ul0-zB+31yK#F|(3%EcH4^gEokYdwg z#FUgN2NL8v6?l=Oe8Vc>4w_G*>uP{xpdq7ab9+ZRoQR2%WS}FI4;AS<60#$(+Bf#(ibdV!5LWf6egNn6q{Ug04Rzs8iEEi z4@EnTfGpOMZvvVWkUlR5Fp6#7j^GXE&2t7;Xzq%(yMl8_l*rT_%)xz&@C3i1R*I%C z2a>36k+mOqhhw!@gM%!j{#p>w!!5M(_VSQ}*Ox9as$N4fXCq*Vj6;AfPUy1{WHQ*) z4eo8~mPFEjBd`}u4+VJBMO(tbYs4dcB0(3@JUa^DtC`3n8VCUs-m*!F7+^s;N!u7O zk6NEhiUB4FoQVNw{F6g5U@@}T#{xG53S&V40>iPu9|523U^N16c|etXupQW--{j&z zAObOQU>yRTaUdLlB|AVU0u4K8i%~oc_Q%sMZUWedIB^MJJpw%mU@ZcRcG8yGonQm9 zs3d|QR7z4J@I;n}iL`4zk5+ex2RT%g>?9D4051uIAn+~;1S8;?3^pNfDH&`*V8$+5 z*x_9?hm%6D9-Trf>`tLw&Z!Vk9X<0Opt_s3r0%8_2JDATVx<8~-1;<_jEbnD0Hsbk zoo-uHI&3uMdYBH^p@3Zm)JuWm8FVRfne-k+Wx^^cS9>NMWyJ@6C}AO=b`A02K2RLL zELa=`TC->i+Bx?~K9NIVc1gnrst_U7vYQdl)x_4KL=!DZq*fPqMN&*jb8UJy& zC5YMN8xqXqj=S2DhXg==3Jm$_0}MZY^v{uG=`Z9V66lfY`+(LI>XohVkKJew-|as5 zt+x9Q4?d412Y$oUjD0{CQGa~331Y@LDsvQ31=YhG(y0`15dFtzoFL4{(c77%>_MPO zx)*~fh}x#pe$5|>AmB`}8Ap91K1St|7mI-^+K^5N7-j4+7l+g-0dmM?19y(GL>jxO z>pz)P0wy6BwW*9jIqZUCK+4bsFcg^hFMX;P3H;@(!%&6`iy*AV$(SoY7Ntzy$fqm( zF+JK9Cg;Y>q8b*RKMbe$_{$Gu!&a1eU)bT&z>`R41 zb@o`b)FEs_5f-G_0f6Qmsd@nBVVY5SEXYu3Mb)I-?bi{)1b#4q{kQ~Nb4NkZ4 z-2lG%w&RrdhLf_DU>13BBaj%kanzeiT{QlGSac*6=86txKqVALL{kQ9@?0e?_ajSHVvT^_86t0pn&Ywr;6{-J+~~%Gwi$t#_eSnn&=6C1wOsNd316@o#+w!+ih% diff --git a/src/main/java/frc/team555/robot/Robot.java b/src/main/java/frc/team555/robot/Robot.java index 7c548cb..c8785c1 100644 --- a/src/main/java/frc/team555/robot/Robot.java +++ b/src/main/java/frc/team555/robot/Robot.java @@ -1,15 +1,9 @@ package frc.team555.robot; -<<<<<<< HEAD import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.test.TestRobot; import frc.team555.robot.test.TestRobot2; public class Robot extends PowerUpRobot { -======= -import frc.team555.robot.test.EncoderTestRobot; - -public class Robot extends EncoderTestRobot { ->>>>>>> josh-auto-switch } diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitch.java b/src/main/java/frc/team555/robot/auto/AutoSwitch.java deleted file mode 100644 index 44325b4..0000000 --- a/src/main/java/frc/team555/robot/auto/AutoSwitch.java +++ /dev/null @@ -1,19 +0,0 @@ -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 { - 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]); - } -} diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java deleted file mode 100644 index 2a23ac5..0000000 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.team555.robot.auto; - -import frc.team555.robot.utils.Side; -import org.montclairrobotics.sprocket.states.State; - -class AutoSwitchLR extends AutoSwitch { - AutoSwitchLR(Side pos, Side target) { - super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")" - - ); - } -} diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java deleted file mode 100644 index edb595c..0000000 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.team555.robot.auto; - -import frc.team555.robot.components.IntakeLift; -import frc.team555.robot.core.Hardware; -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.DriveEncoderLock; -import org.montclairrobotics.sprocket.auto.states.ResetGyro; -import org.montclairrobotics.sprocket.geometry.Angle; - -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, 0.75, Angle.ZERO, true, PowerUpRobot.correction), - /* Turn π/2 radians (in the correct directon) */ - new DriveEncoderGyro(0,(target == Side.RIGHT ? +1 : -1) * 0.75, Angle.QUARTER, true, PowerUpRobot.correction), - /* Drive forward 36 inches */ - new DriveEncoderGyro(36, 0.75, Angle.ZERO, true, PowerUpRobot.correction), - /* Move intake lift up 8 inches */ - new IntakeLiftMove(8, 0.75, true), - /* Turn π/2 radians (in the opposite direction) */ - new DropCubeSwitch(target, PowerUpRobot.correction) - ); - } -} diff --git a/src/main/java/frc/team555/robot/auto/OldDropCube.java b/src/main/java/frc/team555/robot/auto/DropCube.java similarity index 83% rename from src/main/java/frc/team555/robot/auto/OldDropCube.java rename to src/main/java/frc/team555/robot/auto/DropCube.java index e495c47..95beccd 100644 --- a/src/main/java/frc/team555/robot/auto/OldDropCube.java +++ b/src/main/java/frc/team555/robot/auto/DropCube.java @@ -13,15 +13,8 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; -<<<<<<< HEAD:src/main/java/frc/team555/robot/auto/DropCube.java public class DropCube extends StateMachine{ public DropCube(MainLift mainLift,CubeIntake intake, GyroCorrection correction, Input side){ -======= - -@Deprecated -public class OldDropCube extends StateMachine{ - public OldDropCube(CubeIntake intake, MainLift lift, GyroCorrection correction, Side side){ ->>>>>>> josh-auto-switch:src/main/java/frc/team555/robot/auto/OldDropCube.java super( new MultiState( new MoveLift(mainLift,MainLift.TOP*0.6,1,true), diff --git a/src/main/java/frc/team555/robot/auto/DropCubeScale.java b/src/main/java/frc/team555/robot/auto/DropCubeScale.java deleted file mode 100644 index ae76c9f..0000000 --- a/src/main/java/frc/team555/robot/auto/DropCubeScale.java +++ /dev/null @@ -1,10 +0,0 @@ -package frc.team555.robot.auto; - -import org.montclairrobotics.sprocket.states.MultiState; -import org.montclairrobotics.sprocket.states.StateMachine; - -public class DropCubeScale extends StateMachine { - public DropCubeScale() { - super(new MultiState()); - } -} diff --git a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java deleted file mode 100644 index d4c53dc..0000000 --- a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.team555.robot.auto; - -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; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.states.MultiState; -import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.states.StateMachine; - -public class DropCubeSwitch extends StateMachine { - - private CubeIntake intake; - private IntakeLift lift; - - public DropCubeSwitch(Side 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(PowerUpRobot.intake, 1)); - } -} diff --git a/src/main/java/frc/team555/robot/auto/IntakeLiftMove.java b/src/main/java/frc/team555/robot/auto/IntakeLiftMove.java deleted file mode 100644 index bfdceeb..0000000 --- a/src/main/java/frc/team555/robot/auto/IntakeLiftMove.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.team555.robot.auto; - -import frc.team555.robot.core.Hardware; -import org.montclairrobotics.sprocket.states.State; - -public class IntakeLiftMove implements State{ - private double height; - private double power; - private boolean up; - - public IntakeLiftMove(double height, double power,boolean up) - { - this.height=height; - this.power=power; - this.up=up; - } - - - @Override - public void start() { - - } - - @Override - public void stop() - { - Hardware.motorLiftIntake.set(0); - } - - @Override - public void stateUpdate() { - Hardware.motorLiftIntake.set(power); - - } - - @Override - public boolean isDone() { - return Hardware.intakeLiftEncoder.getInches().get()>height==up; - } -} diff --git a/src/main/java/frc/team555/robot/auto/OldSideAuto.java b/src/main/java/frc/team555/robot/auto/OldSideAuto.java index 1550c71..a8dd592 100644 --- a/src/main/java/frc/team555/robot/auto/OldSideAuto.java +++ b/src/main/java/frc/team555/robot/auto/OldSideAuto.java @@ -1,11 +1,12 @@ package frc.team555.robot.auto; +import edu.wpi.first.wpilibj.interfaces.Gyro; +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.core.PowerUpRobot; import frc.team555.robot.utils.Side; +import frc.team555.robot.core.Hardware; import org.montclairrobotics.sprocket.auto.states.*; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; @@ -14,13 +15,14 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; +import org.montclairrobotics.sprocket.utils.PID; import java.util.ArrayList; +@Deprecated public class OldSideAuto implements State { CubeIntake intake; IntakeLift intakeLift; - MainLift mainLift; StateMachine machine; ArrayList states = new ArrayList(); Input crossover; @@ -28,10 +30,9 @@ public class OldSideAuto implements State { GyroCorrection correction; - public OldSideAuto(CubeIntake intake, IntakeLift intakeLift, MainLift mainLift, GyroCorrection gyroCorrection){ + public OldSideAuto(CubeIntake intake, IntakeLift intakeLift, GyroCorrection gyroCorrection){ this.intake = intake; this.intakeLift = intakeLift; - this.mainLift = mainLift; correction = gyroCorrection; @@ -61,10 +62,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] == Side.RIGHT){ states.add(new DriveEncoderGyro(168, .75, Angle.ZERO, false, correction)); - states.add(new DropCubeSwitch(Side.fromDriverStation()[0], correction)); + states.add(new TurnGyro(new Degrees(90), correction, false)); Debug.msg("Target", "RightSwitch"); + //states.add(new Enable(intake)); + //states.add(new Delay(1)); + //states.add(new Disable(intake)); }else if(crossover.get()){ states.add(new DriveEncoderGyro(100, .75, Angle.ZERO, false, correction)); states.add(new DriveEncoderGyro(150, .75, new Degrees(90), false, correction)); diff --git a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java index 95a0282..6c5b03a 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java @@ -15,11 +15,7 @@ public SetIntakeRotation(CubeIntake intake, int position) { @Override public void start() { -<<<<<<< HEAD // intake.roationalMotor.setTarget(position); -======= - intake.rotationalMotor.setTarget(position); ->>>>>>> josh-auto-switch } @Override diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 48c05ae..e4a5e15 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -3,13 +3,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.components.CubeIntake; -<<<<<<< HEAD import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; -======= -import frc.team555.robot.components.MainLift; -import frc.team555.robot.components.SimpleIntake; ->>>>>>> josh-auto-switch import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.auto.states.Delay; @@ -20,29 +15,26 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; -@Deprecated + public class SwitchAuto extends StateMachine{ static Input startSide; + public static SendableChooser startSidesChooser; public static void init(){ -<<<<<<< HEAD startSidesChooser = new SendableChooser<>(); for(Side side : Side.values()){ startSidesChooser.addObject(side.toString(), side); } //SmartDashboard.putData(startSidesChooser); -======= ->>>>>>> josh-auto-switch startSide = new Input() { @Override public Boolean get() { - return Side.fromDriverStation()[0] == PowerUpRobot.startSidesChooser.getSelected(); + return Side.fromDriverStation()[0] == startSidesChooser.getSelected(); } }; } -<<<<<<< HEAD public static void disabled(){ SmartDashboard.putData("Start Position", startSidesChooser); } @@ -65,15 +57,5 @@ public Boolean get() { return !SwitchAuto.startSide.get(); } })); -======= - public static Boolean getStartSide(){ - return startSide.get(); - } - - public SwitchAuto(GyroCorrection correction, CubeIntake intake, MainLift lift){ - super(new ResetGyro(correction), - new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)); - // new ConditionalState(new DropCube(intake, lift, PowerUpRobot.startSidesChooser.getSelected(), startSide)); ->>>>>>> josh-auto-switch } } diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 6e779d2..bcbc962 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -12,7 +12,6 @@ import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; @@ -22,7 +21,6 @@ public class CubeIntake implements Updatable, Togglable{ -<<<<<<< HEAD private double rotatePower=.1; private boolean auto; @@ -31,20 +29,12 @@ public class CubeIntake implements Updatable, Togglable{ public Motor right; public TargetMotor rotate; public final double tolerance = 1; // Todo: Maybe needs to be tuned -======= - 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 ->>>>>>> josh-auto-switch public final int upPos = 0; public final int downPos = 2; public final int middlePos = 1; -<<<<<<< HEAD public Input power; public CubeIntake() { @@ -57,23 +47,6 @@ public CubeIntake() { this.rotate=new TargetMotor(Hardware.intakeRotationEncoder,new BangBang(tolerance,rotatePower),rotateMotor); // this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented -======= - public final 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 - ->>>>>>> josh-auto-switch this.power = new Input() { @Override public Vector get() { @@ -90,7 +63,6 @@ public Vector get() { return new XY(x,y); } }; -<<<<<<< HEAD Control.intakeRotateDown.setPressAction(new ButtonAction() { @@ -105,39 +77,16 @@ public void onAction() { @Override public void onAction() { rotate.setTarget(upPos); -======= - Control.intakeRotationDown.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(downPos); - } - }); - Control.intakeRotationUp.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(upPos); ->>>>>>> josh-auto-switch } }); Control.intakeRotateMiddle.setPressAction(new ButtonAction() { @Override public void onAction() { -<<<<<<< HEAD rotate.setTarget(middlePos); -======= - rotationalMotor.set(middlePos); ->>>>>>> josh-auto-switch } }); - Control.intakeLiftManualUp.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(rotationalMotorPower); - } - }); -<<<<<<< HEAD Control.intakeRotateUpManual.setPressAction(new ButtonAction() { @Override public void onAction() { @@ -165,37 +114,6 @@ public void onAction() { }); rotate.setPower(0); -======= - Control.intakeLiftManualUp.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(rotationalMotorPower); - } - }); - - - new CurrentMonitor("Intake Right Motor", Hardware.motorIntakeR, new Input() { - @Override - public Boolean get() { - return power.get().getMagnitude() > .1; - } - }); - - new CurrentMonitor("Intake Left Motor", Hardware.motorIntakeL, new Input() { - @Override - public Boolean get() { - return power.get().getMagnitude() > .1; - } - }); - - new CurrentMonitor("Intake Rotational Motor", Hardware.motorRotational, new Input() { - @Override - public Boolean get() { - return Math.abs(rotationalMotor.getTarget() - rotationalMotor.getDistance().get()) > 20; - } - }).setEncoder(Hardware.intakeRotationEncoder); - ->>>>>>> josh-auto-switch Updater.add(this, Priority.CALC); } @@ -203,7 +121,6 @@ public Boolean get() { @Override public void update() { Vector p = power.get(); -<<<<<<< HEAD if(!auto) { left.set(p.getY() - p.getX()*.25); right.set(p.getY() + p.getX()*.25); @@ -224,17 +141,6 @@ public void setAutoPower(double power){ auto = true; left.set(power); right.set(power); -======= - // if(p.getMagnitude() < tolerance){ - // openClamp(); - // }else{ - // closeClamp(); - // } - left.set(p.getX()); - right.set(p.getY()); - - // updateClamp(); ->>>>>>> josh-auto-switch } @Override diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 20a2826..9b744be 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -5,10 +5,8 @@ import frc.team555.robot.utils.BangBang; import frc.team555.robot.utils.TargetMotor; import org.montclairrobotics.sprocket.control.ButtonAction; -import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; -import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.Utils; @@ -32,11 +30,7 @@ public class IntakeLift implements Lift{ * Constructor for IntakeLift Class with default position of 0 */ public IntakeLift() { -<<<<<<< HEAD motors = new TargetMotor(Hardware.intakeLiftEncoder, new BangBang(1,1),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng -======= - motors = new TargetMotor(Hardware.liftEncoder, new PID(0, 0, 0),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng ->>>>>>> josh-auto-switch encoder=Hardware.intakeLiftEncoder; //================= @@ -101,22 +95,9 @@ public void onAction() { motors.setPower(0); } }; -<<<<<<< HEAD Control.intakeLiftManualUp.setReleaseAction(stop); Control.intakeLiftManualDown.setReleaseAction(stop); motors.setPower(0); -======= - Control.intakeLiftManualUp.setOffAction(stop); - Control.intakeLiftManualDown.setOffAction(stop); - - new CurrentMonitor("Intake Lift Encoder", Hardware.motorLiftIntake, new Input() { - @Override - public Boolean get() { - return Control.intakeLiftManualUp.get() || Control.intakeLiftManualUp.get() || Math.abs(motors.getTarget() - motors.getDistance().get()) > 20; - } - }).setEncoder(Hardware.intakeLiftEncoder); - ->>>>>>> josh-auto-switch } diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 94812e4..3c65752 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -1,33 +1,20 @@ package frc.team555.robot.components; -<<<<<<< HEAD import frc.team555.robot.auto.MoveLift; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.BangBang; -======= -import edu.wpi.first.wpilibj.DigitalInput; -import frc.team555.robot.core.Control; -import frc.team555.robot.core.Hardware; -import frc.team555.robot.utils.MotorMonitor; ->>>>>>> josh-auto-switch 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; -<<<<<<< HEAD import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.states.State; import org.montclairrobotics.sprocket.states.StateMachine; -======= -import org.montclairrobotics.sprocket.motors.CurrentMonitor; -import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.utils.Input; ->>>>>>> josh-auto-switch import org.montclairrobotics.sprocket.utils.PID; import java.nio.channels.Pipe; @@ -42,14 +29,9 @@ public class MainLift extends TargetMotor implements Lift { private int upPosition; private int downPosition; - private DigitalInput bottomLimitSwitch; public MainLift(){ -<<<<<<< HEAD super(Hardware.liftEncoder, new BangBang(1,1), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); -======= - super(Hardware.liftEncoder, new PID(.1, 0, 0), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); ->>>>>>> josh-auto-switch mode = Mode.POWER; @@ -72,7 +54,6 @@ public void onAction() { Control.mainLiftManualDown.setHeldAction(new ButtonAction() { @Override public void onAction() { -<<<<<<< HEAD if(LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) { if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); @@ -83,29 +64,14 @@ public void onAction() { }else{ set(0); encoder.reset(); -======= - if(!bottomLimitSwitch.get()) { - MainLift.super.set(-speed); - }else{ - MainLift.super.set(0); - Hardware.liftEncoder.reset(); ->>>>>>> josh-auto-switch } } }); - - Control.mainLiftManualDown.setReleaseAction(new ButtonAction() { @Override public void onAction() { -<<<<<<< HEAD set(0); -======= - if(encoder != null) { - MainLift.super.set(0); - } ->>>>>>> josh-auto-switch } }); @@ -133,7 +99,6 @@ public void onAction() { } } }); -<<<<<<< HEAD Control.mainLiftAutoUp.setReleaseAction(new ButtonAction() { @Override public void onAction() { @@ -142,26 +107,6 @@ public void onAction() { } }); setPower(0); -======= - - - - new CurrentMonitor("Main Lift Front", Hardware.motorLiftMainFront, new Input() { - @Override - public Boolean get() { - return Math.abs(getTarget() - getDistance().get()) > 20 || speed != 0; - } - }).setEncoder(Hardware.liftEncoder); - - 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); - - ->>>>>>> josh-auto-switch // Lift Bottom diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 8fb8df5..28b1c3b 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -29,7 +29,6 @@ public class Control { mainLiftAutoUp, mainLiftManualUp, mainLiftManualDown, -<<<<<<< HEAD // mainLiftTop, // mainLiftBottom, //openButton, @@ -45,22 +44,6 @@ public class Control { //public static int FieldCentricID=2; //public static int ResetID=3; -======= - mainLiftTop, - mainLiftBottom, - openButton, - closeButton, - intakeRotationUp, - intakeRotationDown, - intakeRotationMiddle, - intakeSubroutine, - intakeRotationManualUp, - intakeRotationManualDown; - - - public static int FieldCentricID=2; - public static int ResetID=3; ->>>>>>> josh-auto-switch // Joystick Declaration public static Joystick driveStick; public static Joystick auxStick; @@ -89,7 +72,6 @@ public static void init() { // intakeLiftIncrement = new JoystickButton(auxStick,12); // intakeLiftDecrement = new JoystickButton(auxStick,12); -<<<<<<< HEAD // mainLiftTop = new JoystickButton(auxStick,9); // mainLiftBottom = new JoystickButton(auxStick,8); @@ -116,20 +98,6 @@ public static void init() { // intakeLiftBottom = new JoystickButton(auxStick, 13); // intakeSubroutine = new JoystickButton(auxStick, 14); -======= - // mainLiftTop = new JoystickButton(auxStick,12); - // mainLiftBottom = new JoystickButton(auxStick,12); - mainLiftManualUp = new JoystickButton(auxStick,11); - mainLiftManualDown = new JoystickButton(auxStick,10); - intakeLiftManualUp = new JoystickButton(auxStick,3); - intakeLiftManualDown = new JoystickButton(auxStick,2); - intakeLiftTop = new JoystickButton(auxStick, 6); - intakeLiftBottom = new JoystickButton(auxStick, 7); - intakeSubroutine = new JoystickButton(auxStick, 14); - intakeRotationUp = new JoystickButton(auxStick,8); - intakeRotationManualUp = new JoystickButton(auxStick, 9); - intakeRotationManualDown = new JoystickButton(auxStick, 8); ->>>>>>> josh-auto-switch // openButton=new JoystickButton(Control.auxStick,8); // closeButton=new JoystickButton(Control.auxStick,9); diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 3778714..83d72f4 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -63,13 +63,6 @@ private static class DeviceID { public static final int motorIntakeL = 10; public static final int motorIntakeR = 5; -<<<<<<< HEAD -======= - public static final int motorRotational = 10; - - public static final int motorIntakeL = 5; - public static final int motorIntakeR = 10; ->>>>>>> josh-auto-switch //left intake 5 //pincer 6 //lifter 9 @@ -147,9 +140,6 @@ public static void init(){ motorIntakeR = new WPI_TalonSRX(DeviceID.motorIntakeR); motorIntakeL = new WPI_TalonSRX(DeviceID.motorIntakeL); - motorIntakeL = new WPI_TalonSRX(DeviceID.motorIntakeL); - motorIntakeR = new WPI_TalonSRX(DeviceID.motorIntakeR); - motorLiftMainBack.setInverted(true); //motorIntakeClamp = new WPI_TalonSRX(DeviceID.motorIntakeClamp); @@ -182,15 +172,9 @@ public static void init(){ //Intake Lift Travel //15.5 to 49.5 -<<<<<<< HEAD liftEncoder = new SEncoder(new Encoder(4,5), 1/*31600/(82 - 54)*/); // todo: REALLY NEED TO BE SET intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1 /*1333600/(49.5 - 15.5)*/); intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // todo, needs to be set -======= - liftEncoder = new SEncoder(new Encoder(4,5), 31600/(82 - 54)); // todo: REALLY NEED TO BE SET - intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1333600/(49.5 - 15.5)); - intakeRotationEncoder = new TalonEncoder(motorRotational, 1); // todo, needs to be set ->>>>>>> josh-auto-switch navx = new NavXInput(DeviceID.navxPort); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index b0db476..80d6cb0 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -1,22 +1,15 @@ package frc.team555.robot.core; -<<<<<<< HEAD import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; -======= ->>>>>>> josh-auto-switch import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; -<<<<<<< HEAD import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.CoastMotor; -======= -import frc.team555.robot.utils.MotorMonitor; ->>>>>>> josh-auto-switch import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; @@ -28,7 +21,6 @@ import org.montclairrobotics.sprocket.drive.steps.Sensitivity; import org.montclairrobotics.sprocket.drive.utils.GyroLock; import org.montclairrobotics.sprocket.geometry.*; -import org.montclairrobotics.sprocket.motors.CurrentMonitor; import org.montclairrobotics.sprocket.motors.Module; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.pipeline.Step; @@ -43,7 +35,6 @@ public class PowerUpRobot extends SprocketRobot { DriveTrain driveTrain; public static GyroCorrection correction; -<<<<<<< HEAD Sensitivity sensitivity; GyroLock lock; boolean manualLock; @@ -54,13 +45,6 @@ public class PowerUpRobot extends SprocketRobot { public static Side startSide; //SendableChooser startSideChooser; static Input switchOnSide; -======= - GyroLock lock; - boolean manualLock; - public static CubeIntake intake; - - public static SendableChooser startSidesChooser; ->>>>>>> josh-auto-switch //vision stuff @@ -120,33 +104,6 @@ public Boolean get() { new CoastMotor(Hardware.motorDriveBR,false), new CoastMotor(Hardware.motorDriveFR,false)); - new CurrentMonitor("Front Right Drive Motor", Hardware.motorDriveFR, new Input() { - @Override - public Boolean get() { - return Control.driveStick.getMagnitude() > .1; - } - }).setEncoder(Hardware.rightDriveEncoder); - - new CurrentMonitor("Back Right Drive Motor", Hardware.motorDriveBR, new Input() { - @Override - public Boolean get() { - return Control.driveStick.getMagnitude() > .1; - } - }).setEncoder(Hardware.rightDriveEncoder); - - new CurrentMonitor("Front Left Drive Motor", Hardware.motorDriveFL, new Input() { - @Override - public Boolean get() { - return Control.driveStick.getMagnitude() > .1; - } - }).setEncoder(Hardware.leftDriveEncoder); - - new CurrentMonitor("Back Left Drive Motor", Hardware.motorDriveBL, new Input() { - @Override - public Boolean get() { - return Control.driveStick.getMagnitude() > .1; - } - }).setEncoder(Hardware.leftDriveEncoder); DriveTrainBuilder dtBuilder = new DriveTrainBuilder(); @@ -269,9 +226,6 @@ public void onAction() { correction)); - - - AutoMode encoder = new AutoMode("encoder", new DriveEncoders(100,.25)); @@ -378,7 +332,7 @@ public void onAction() { public void onAction() { shootCube.stop(); intake.disable(); - intake.rotationalMotor.set(intake.downPos); + intake.roationalMotor.set(intake.downPos); } }); */ @@ -421,17 +375,7 @@ public void onAction() { } }); visionThread.start();*/ -<<<<<<< HEAD // intakeLift.setPower(0); -======= - - startSidesChooser = new SendableChooser<>(); - for(Side side : Side.values()){ - startSidesChooser.addObject(side.toString(), side); - } - SmartDashboard.putData(startSidesChooser); - ->>>>>>> josh-auto-switch } @Override @@ -456,7 +400,6 @@ public void update(){ SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); -<<<<<<< HEAD SmartDashboard.putBoolean("Lift Limit Switch", Hardware.liftLimitSwitch.get()); SmartDashboard.putNumber("POV",Control.auxStick.getPOV()); debugCurrent("Main Lift Front",Hardware.motorLiftMainFront); @@ -464,10 +407,6 @@ public void update(){ debugCurrent("Intake Lift",Hardware.motorLiftIntake); SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); -======= - Debug.msg("Intake Rotation", Hardware.intakeRotationEncoder.get()); - ->>>>>>> josh-auto-switch gyroLocking(); //startSide = startSideChooser.getSelected(); } @@ -497,12 +436,8 @@ private void gyroLocking(){ @Override public void userDisabledPeriodic(){ -<<<<<<< HEAD //SmartDashboard.putData(startSideChooser); //startSide = startSideChooser.getSelected(); SwitchAuto.disabled(); -======= - SmartDashboard.putData(startSidesChooser) ; ->>>>>>> josh-auto-switch } } diff --git a/src/main/java/frc/team555/robot/test/EncoderTestRobot.java b/src/main/java/frc/team555/robot/test/EncoderTestRobot.java deleted file mode 100644 index 6dfcfa9..0000000 --- a/src/main/java/frc/team555/robot/test/EncoderTestRobot.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.team555.robot.test; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.Joystick; -import frc.team555.robot.utils.TalonEncoder; -import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.drive.ControlledMotor; -import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.Input; - -public class EncoderTestRobot extends SprocketRobot { - - TalonEncoder testEncoder; - WPI_TalonSRX testMotorController; - ControlledMotor testMotor; - Joystick testJoystick; - - @Override - public void robotInit() { - testMotorController = new WPI_TalonSRX(0); - testEncoder = new TalonEncoder(testMotorController, 1); - testJoystick = new Joystick(0); - testMotor = new ControlledMotor(testMotorController, new Input() { - @Override - public Double get() { - return testJoystick.getY(); - } - }); - } - @Override - public void update() { - Debug.msg("Motor Position",testEncoder.get()); - Debug.msg("Motor Speed",testEncoder.getSpeed()); - } - -} diff --git a/src/main/java/frc/team555/robot/utils/TargetMotor.java b/src/main/java/frc/team555/robot/utils/TargetMotor.java index bfaf839..1ab2bfa 100644 --- a/src/main/java/frc/team555/robot/utils/TargetMotor.java +++ b/src/main/java/frc/team555/robot/utils/TargetMotor.java @@ -83,8 +83,4 @@ public void update() { super.set(pid.get()); } } - - public double getTarget(){ - return pid.getTarget(); - } } From 8eb02850f2b328a97632e4b4190f7d01e56f4551 Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 14:05:35 -0400 Subject: [PATCH 29/38] Merge Cleanup Branch and Josh Auto branch --- .../{OldDropCube.java => SimpleDropCube.java} | 10 +- .../frc/team555/robot/auto/SwitchAuto.java | 24 ++--- .../team555/robot/components/CubeIntake.java | 95 +------------------ .../java/frc/team555/robot/core/Control.java | 34 +------ .../frc/team555/robot/core/PowerUpRobot.java | 47 +-------- 5 files changed, 15 insertions(+), 195 deletions(-) rename src/main/java/frc/team555/robot/auto/{OldDropCube.java => SimpleDropCube.java} (76%) diff --git a/src/main/java/frc/team555/robot/auto/OldDropCube.java b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java similarity index 76% rename from src/main/java/frc/team555/robot/auto/OldDropCube.java rename to src/main/java/frc/team555/robot/auto/SimpleDropCube.java index 3f1a3eb..090fe34 100644 --- a/src/main/java/frc/team555/robot/auto/OldDropCube.java +++ b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java @@ -13,15 +13,9 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; -<<<<<<< HEAD:src/main/java/frc/team555/robot/auto/OldDropCube.java -@Deprecated -public class OldDropCube extends StateMachine{ - public OldDropCube(CubeIntake intake, MainLift lift, GyroCorrection correction, Side side){ -======= -public class DropCube extends StateMachine{ - public DropCube(MainLift mainLift,CubeIntake intake, GyroCorrection correction, Input side){ ->>>>>>> cleanup:src/main/java/frc/team555/robot/auto/DropCube.java +public class SimpleDropCube extends StateMachine{ + public SimpleDropCube(MainLift mainLift, CubeIntake intake, GyroCorrection correction, Input side){ super( new MultiState( new MoveLift(mainLift,MainLift.TOP*0.6,1,true), diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 70b994a..923efcd 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -3,13 +3,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.components.CubeIntake; -<<<<<<< HEAD -import frc.team555.robot.components.MainLift; -import frc.team555.robot.components.SimpleIntake; -======= import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; ->>>>>>> cleanup import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.auto.states.Delay; @@ -20,38 +15,32 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; + + @Deprecated public class SwitchAuto extends StateMachine{ static Input startSide; + public static SendableChooserstartSidesChooser; public static void init(){ -<<<<<<< HEAD -======= - startSidesChooser = new SendableChooser<>(); + startSidesChooser = new SendableChooser(); for(Side side : Side.values()){ startSidesChooser.addObject(side.toString(), side); } //SmartDashboard.putData(startSidesChooser); ->>>>>>> cleanup startSide = new Input() { @Override public Boolean get() { - return Side.fromDriverStation()[0] == PowerUpRobot.startSidesChooser.getSelected(); + return Side.fromDriverStation()[0] == startSidesChooser.getSelected(); } }; } -<<<<<<< HEAD public static Boolean getStartSide(){ return startSide.get(); } - public SwitchAuto(GyroCorrection correction, CubeIntake intake, MainLift lift){ - super(new ResetGyro(correction), - new DriveEncoderGyro(150, .75, Angle.ZERO, false, correction)); - // new ConditionalState(new DropCube(intake, lift, PowerUpRobot.startSidesChooser.getSelected(), startSide)); -======= public static void disabled(){ SmartDashboard.putData("Start Position", startSidesChooser); } @@ -62,7 +51,7 @@ public static void loop(){ public SwitchAuto(MainLift mainLift, GyroCorrection correction, CubeIntake intake){ super(new ResetGyro(correction), - new ConditionalState(new DropCube(mainLift,intake, correction, new Input(){ + new ConditionalState(new SimpleDropCube(mainLift,intake, correction, new Input(){ @Override public Side get() { return SwitchAuto.startSidesChooser.getSelected(); @@ -74,6 +63,5 @@ public Boolean get() { return !SwitchAuto.startSide.get(); } })); ->>>>>>> cleanup } } diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 930bed0..8f8e07c 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -22,13 +22,6 @@ public class CubeIntake implements Updatable, Togglable{ -<<<<<<< HEAD - 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 rotatePower=.1; private boolean auto; @@ -37,30 +30,12 @@ public class CubeIntake implements Updatable, Togglable{ public Motor right; public TargetMotor rotate; public final double tolerance = 1; // Todo: Maybe needs to be tuned ->>>>>>> cleanup public final int upPos = 0; public final int downPos = 2; public final int middlePos = 1; -<<<<<<< HEAD - public final 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 - -======= public Input power; public CubeIntake() { @@ -73,7 +48,6 @@ public CubeIntake() { this.rotate=new TargetMotor(Hardware.intakeRotationEncoder,new BangBang(tolerance,rotatePower),rotateMotor); // this.roationalMotor = new TargetMotor(Hardware.intakeRotationEncoder, new BangBang(tolerance,rotatePower), rotateMotor); // Todo: needs to be implemented ->>>>>>> cleanup this.power = new Input() { @Override public Vector get() { @@ -90,18 +64,7 @@ public Vector get() { return new XY(x,y); } }; -<<<<<<< HEAD - Control.intakeRotationDown.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(downPos); - } - }); - Control.intakeRotationUp.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(upPos); -======= + Control.intakeRotateDown.setPressAction(new ButtonAction() { @@ -116,58 +79,18 @@ public void onAction() { @Override public void onAction() { rotate.setTarget(upPos); ->>>>>>> cleanup } }); Control.intakeRotateMiddle.setPressAction(new ButtonAction() { @Override public void onAction() { -<<<<<<< HEAD - rotationalMotor.set(middlePos); -======= - rotate.setTarget(middlePos); ->>>>>>> cleanup - } - }); - - Control.intakeLiftManualUp.setPressAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(rotationalMotorPower); - } - }); - -<<<<<<< HEAD - Control.intakeLiftManualUp.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - rotationalMotor.set(rotationalMotorPower); - } - }); - - new CurrentMonitor("Intake Right Motor", Hardware.motorIntakeR, new Input() { - @Override - public Boolean get() { - return power.get().getMagnitude() > .1; + rotate.setTarget(middlePos); } }); - new CurrentMonitor("Intake Left Motor", Hardware.motorIntakeL, new Input() { - @Override - public Boolean get() { - return power.get().getMagnitude() > .1; - } - }); - new CurrentMonitor("Intake Rotational Motor", Hardware.motorRotational, new Input() { - @Override - public Boolean get() { - return Math.abs(rotationalMotor.getTarget() - rotationalMotor.getDistance().get()) > 20; - } - }).setEncoder(Hardware.intakeRotationEncoder); -======= Control.intakeRotateUpManual.setPressAction(new ButtonAction() { @Override public void onAction() { @@ -195,7 +118,6 @@ public void onAction() { }); rotate.setPower(0); ->>>>>>> cleanup Updater.add(this, Priority.CALC); } @@ -203,17 +125,7 @@ public void onAction() { @Override public void update() { Vector p = power.get(); -<<<<<<< HEAD - // 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); right.set(p.getY() + p.getX()*.25); @@ -234,7 +146,6 @@ public void setAutoPower(double power){ auto = true; left.set(power); right.set(power); ->>>>>>> cleanup } @Override diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 7cf7a1d..8153f49 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -29,22 +29,7 @@ public class Control { mainLiftAutoUp, mainLiftManualUp, mainLiftManualDown, -<<<<<<< HEAD - mainLiftTop, - mainLiftBottom, - openButton, - closeButton, - intakeRotationUp, - intakeRotationDown, - intakeRotationMiddle, - intakeSubroutine, - intakeRotationManualUp, - intakeRotationManualDown; - - - public static int FieldCentricID=2; - public static int ResetID=3; -======= + // mainLiftTop, // mainLiftBottom, //openButton, @@ -60,7 +45,6 @@ public class Control { //public static int FieldCentricID=2; //public static int ResetID=3; ->>>>>>> cleanup // Joystick Declaration public static Joystick driveStick; public static Joystick auxStick; @@ -89,20 +73,7 @@ public static void init() { // intakeLiftIncrement = new JoystickButton(auxStick,12); // intakeLiftDecrement = new JoystickButton(auxStick,12); -<<<<<<< HEAD - // mainLiftTop = new JoystickButton(auxStick,12); - // mainLiftBottom = new JoystickButton(auxStick,12); - mainLiftManualUp = new JoystickButton(auxStick,11); - mainLiftManualDown = new JoystickButton(auxStick,10); - intakeLiftManualUp = new JoystickButton(auxStick,3); - intakeLiftManualDown = new JoystickButton(auxStick,2); - intakeLiftTop = new JoystickButton(auxStick, 6); - intakeLiftBottom = new JoystickButton(auxStick, 7); - intakeSubroutine = new JoystickButton(auxStick, 14); - intakeRotationUp = new JoystickButton(auxStick,8); - intakeRotationManualUp = new JoystickButton(auxStick, 9); - intakeRotationManualDown = new JoystickButton(auxStick, 8); -======= + // mainLiftTop = new JoystickButton(auxStick,9); // mainLiftBottom = new JoystickButton(auxStick,8); @@ -129,7 +100,6 @@ public static void init() { // intakeLiftBottom = new JoystickButton(auxStick, 13); // intakeSubroutine = new JoystickButton(auxStick, 14); ->>>>>>> cleanup // openButton=new JoystickButton(Control.auxStick,8); // closeButton=new JoystickButton(Control.auxStick,9); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 4d23f58..f1bc339 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -1,22 +1,13 @@ package frc.team555.robot.core; -<<<<<<< HEAD -======= + import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; ->>>>>>> cleanup -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; -<<<<<<< HEAD -import frc.team555.robot.utils.MotorMonitor; -======= -import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.CoastMotor; ->>>>>>> cleanup import frc.team555.robot.utils.Side; import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoMode; @@ -43,13 +34,7 @@ public class PowerUpRobot extends SprocketRobot { DriveTrain driveTrain; public static GyroCorrection correction; -<<<<<<< HEAD - GyroLock lock; - boolean manualLock; - public static CubeIntake intake; - public static SendableChooser startSidesChooser; -======= Sensitivity sensitivity; GyroLock lock; boolean manualLock; @@ -57,10 +42,7 @@ public class PowerUpRobot extends SprocketRobot { MainLift mainLift; // IntakeLift intakeLift; StateMachine autoClimb; - public static Side startSide; //SendableChooser startSideChooser; - static Input switchOnSide; ->>>>>>> cleanup //vision stuff @@ -349,26 +331,20 @@ public void onAction() { addAutoMode(baseLine); addAutoMode(centerBaseLineLeft); addAutoMode(centerBaseLineRight); -<<<<<<< HEAD - /* 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))); ->>>>>>> cleanup //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() { @Override @@ -433,17 +409,6 @@ public void onAction() { } }); visionThread.start();*/ -<<<<<<< HEAD - - startSidesChooser = new SendableChooser<>(); - for(Side side : Side.values()){ - startSidesChooser.addObject(side.toString(), side); - } - SmartDashboard.putData(startSidesChooser); - -======= - // intakeLift.setPower(0); ->>>>>>> cleanup } @Override @@ -468,10 +433,7 @@ public void update(){ SmartDashboard.putNumber("Distance", driveTrain.getDistance().getY()); SmartDashboard.putNumber("Left Encoder", Hardware.leftDriveEncoder.getInches().get()); SmartDashboard.putNumber("Right Encoder", Hardware.rightDriveEncoder.getInches().get()); -<<<<<<< HEAD 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); @@ -479,7 +441,6 @@ public void update(){ debugCurrent("Intake Lift",Hardware.motorLiftIntake); SmartDashboard.putNumber("Main Lift Encoder Value",Hardware.liftEncoder.getInches().get()); //SmartDashboard.putNumber("Intake Lift Encoder",in.getInches().get()); ->>>>>>> cleanup gyroLocking(); //startSide = startSideChooser.getSelected(); } @@ -509,12 +470,8 @@ private void gyroLocking(){ @Override public void userDisabledPeriodic(){ -<<<<<<< HEAD - SmartDashboard.putData(startSidesChooser) ; -======= //SmartDashboard.putData(startSideChooser); //startSide = startSideChooser.getSelected(); SwitchAuto.disabled(); ->>>>>>> cleanup } } From 5b04cc8cb7d1b8fb0063b1eac87f517d80ebfeb2 Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 14:33:32 -0400 Subject: [PATCH 30/38] Build sucessful --- src/main/java/frc/team555/robot/Robot.java | 5 -- .../frc/team555/robot/auto/AutoSwitchLR.java | 8 +-- .../frc/team555/robot/auto/AutoSwitchMid.java | 3 +- .../frc/team555/robot/auto/CubeOuttake.java | 2 +- .../team555/robot/auto/DropCubeSwitch.java | 3 +- .../frc/team555/robot/auto/OldSideAuto.java | 8 ++- .../team555/robot/auto/SetIntakeRotation.java | 4 -- .../frc/team555/robot/auto/SwitchAuto.java | 1 - .../team555/robot/components/IntakeLift.java | 18 +------ .../team555/robot/components/MainLift.java | 53 ++----------------- .../java/frc/team555/robot/core/Hardware.java | 15 +----- .../frc/team555/robot/core/PowerUpRobot.java | 2 +- 12 files changed, 21 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/team555/robot/Robot.java b/src/main/java/frc/team555/robot/Robot.java index f543af6..477300f 100644 --- a/src/main/java/frc/team555/robot/Robot.java +++ b/src/main/java/frc/team555/robot/Robot.java @@ -1,15 +1,10 @@ package frc.team555.robot; -<<<<<<< HEAD -import frc.team555.robot.test.EncoderTestRobot; -public class Robot extends EncoderTestRobot { -======= import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.test.TestRobot; import frc.team555.robot.test.TestRobot2; public class Robot extends PowerUpRobot { ->>>>>>> cleanup } diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java index 69d5019..c8a37e7 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -5,13 +5,10 @@ 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) { -<<<<<<< HEAD - super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")" - -======= super("Cube to " + target.toString() + " Switch (" + pos.toString() + ")", /* Reset gyro */ new ResetGyro(PowerUpRobot.correction), @@ -30,8 +27,7 @@ class AutoSwitchLR extends AutoSwitch { /* 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(pos, PowerUpRobot.correction) ->>>>>>> 5d54d05487e29bdc2cabd1be43e64b5723d14495 + 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 index 1325a11..da82d9c 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -5,6 +5,7 @@ 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 { @@ -19,7 +20,7 @@ class AutoSwitchMid extends AutoSwitch { /* 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(target, PowerUpRobot.correction) + new DropCubeSwitch(new Input(){public Side get(){return target;}}, PowerUpRobot.correction) ); } } diff --git a/src/main/java/frc/team555/robot/auto/CubeOuttake.java b/src/main/java/frc/team555/robot/auto/CubeOuttake.java index d580bb6..ceed9ca 100644 --- a/src/main/java/frc/team555/robot/auto/CubeOuttake.java +++ b/src/main/java/frc/team555/robot/auto/CubeOuttake.java @@ -5,7 +5,7 @@ import org.montclairrobotics.sprocket.auto.states.Disable; import org.montclairrobotics.sprocket.auto.states.Enable; import org.montclairrobotics.sprocket.states.StateMachine; -@Deprecated + public class CubeOuttake extends StateMachine { public CubeOuttake(CubeIntake intake, double time){ diff --git a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java index d4c53dc..14d6c02 100644 --- a/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java +++ b/src/main/java/frc/team555/robot/auto/DropCubeSwitch.java @@ -10,13 +10,14 @@ 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(Side side, GyroCorrection correction){ + public DropCubeSwitch(Input side, GyroCorrection correction){ super( new MultiState(0, new SideTurn(correction,false, side), diff --git a/src/main/java/frc/team555/robot/auto/OldSideAuto.java b/src/main/java/frc/team555/robot/auto/OldSideAuto.java index 1550c71..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(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/SetIntakeRotation.java b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java index 5911700..6c5b03a 100644 --- a/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java +++ b/src/main/java/frc/team555/robot/auto/SetIntakeRotation.java @@ -15,11 +15,7 @@ public SetIntakeRotation(CubeIntake intake, int position) { @Override public void start() { -<<<<<<< HEAD - intake.rotationalMotor.setTarget(position); -======= // intake.roationalMotor.setTarget(position); ->>>>>>> cleanup } @Override diff --git a/src/main/java/frc/team555/robot/auto/SwitchAuto.java b/src/main/java/frc/team555/robot/auto/SwitchAuto.java index 923efcd..8035cbf 100644 --- a/src/main/java/frc/team555/robot/auto/SwitchAuto.java +++ b/src/main/java/frc/team555/robot/auto/SwitchAuto.java @@ -17,7 +17,6 @@ import org.montclairrobotics.sprocket.utils.Input; -@Deprecated public class SwitchAuto extends StateMachine{ static Input startSide; diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 16e83cf..1db9719 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -32,11 +32,9 @@ public class IntakeLift implements Lift{ * Constructor for IntakeLift Class with default position of 0 */ public IntakeLift() { -<<<<<<< HEAD - motors = new TargetMotor(Hardware.liftEncoder, new PID(0, 0, 0),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng -======= + motors = new TargetMotor(Hardware.intakeLiftEncoder, new BangBang(1,1),new Motor(Hardware.motorLiftIntake)); // Todo: Needs Tuninng ->>>>>>> cleanup + encoder=Hardware.intakeLiftEncoder; //================= @@ -101,22 +99,10 @@ public void onAction() { motors.setPower(0); } }; -<<<<<<< HEAD - Control.intakeLiftManualUp.setOffAction(stop); - Control.intakeLiftManualDown.setOffAction(stop); - - new CurrentMonitor("Intake Lift Encoder", Hardware.motorLiftIntake, new Input() { - @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); ->>>>>>> cleanup } diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index c43d8c2..dcd1658 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -1,33 +1,23 @@ package frc.team555.robot.components; -<<<<<<< HEAD + import edu.wpi.first.wpilibj.DigitalInput; -import frc.team555.robot.core.Control; -import frc.team555.robot.core.Hardware; -import frc.team555.robot.utils.MotorMonitor; -======= import frc.team555.robot.auto.MoveLift; import frc.team555.robot.core.Control; import frc.team555.robot.core.Hardware; import frc.team555.robot.core.PowerUpRobot; import frc.team555.robot.utils.BangBang; ->>>>>>> cleanup 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; -<<<<<<< HEAD -import org.montclairrobotics.sprocket.motors.CurrentMonitor; -import org.montclairrobotics.sprocket.motors.Motor; -import org.montclairrobotics.sprocket.utils.Input; -======= + import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.states.State; import org.montclairrobotics.sprocket.states.StateMachine; ->>>>>>> cleanup import org.montclairrobotics.sprocket.utils.PID; import java.nio.channels.Pipe; @@ -45,11 +35,8 @@ public class MainLift extends TargetMotor implements Lift { private DigitalInput bottomLimitSwitch; public MainLift(){ -<<<<<<< HEAD - 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)); ->>>>>>> cleanup mode = Mode.POWER; @@ -72,13 +59,6 @@ public void onAction() { Control.mainLiftManualDown.setHeldAction(new ButtonAction() { @Override public void onAction() { -<<<<<<< HEAD - if(!bottomLimitSwitch.get()) { - MainLift.super.set(-speed); - }else{ - MainLift.super.set(0); - Hardware.liftEncoder.reset(); -======= if(LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) { if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); @@ -89,7 +69,6 @@ public void onAction() { }else{ set(0); encoder.reset(); ->>>>>>> cleanup } } }); @@ -99,13 +78,7 @@ public void onAction() { Control.mainLiftManualDown.setReleaseAction(new ButtonAction() { @Override public void onAction() { -<<<<<<< HEAD - if(encoder != null) { - MainLift.super.set(0); - } -======= set(0); ->>>>>>> cleanup } }); @@ -133,26 +106,7 @@ public void onAction() { } } }); -<<<<<<< HEAD - - - - new CurrentMonitor("Main Lift Front", Hardware.motorLiftMainFront, new Input() { - @Override - public Boolean get() { - return Math.abs(getTarget() - getDistance().get()) > 20 || speed != 0; - } - }).setEncoder(Hardware.liftEncoder); - - 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); - -======= Control.mainLiftAutoUp.setReleaseAction(new ButtonAction() { @Override public void onAction() { @@ -161,7 +115,6 @@ public void onAction() { } }); setPower(0); ->>>>>>> cleanup // Lift Bottom diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 2e0355d..8021782 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -63,13 +63,6 @@ private static class DeviceID { public static final int motorIntakeL = 10; public static final int motorIntakeR = 5; -<<<<<<< HEAD - public static final int motorRotational = 10; - - public static final int motorIntakeL = 5; - public static final int motorIntakeR = 10; -======= ->>>>>>> cleanup //left intake 5 //pincer 6 //lifter 9 @@ -182,15 +175,11 @@ public static void init(){ //Intake Lift Travel //15.5 to 49.5 -<<<<<<< HEAD - liftEncoder = new SEncoder(new Encoder(4,5), 31600/(82 - 54)); // todo: REALLY NEED TO BE SET - intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1333600/(49.5 - 15.5)); - intakeRotationEncoder = new TalonEncoder(motorRotational, 1); // todo, needs to be set -======= + liftEncoder = new SEncoder(new Encoder(4,5), 1/*31600/(82 - 54)*/); // todo: REALLY NEED TO BE SET intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1 /*1333600/(49.5 - 15.5)*/); intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // todo, needs to be set ->>>>>>> cleanup + navx = new NavXInput(DeviceID.navxPort); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index f1bc339..33aba83 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -38,7 +38,7 @@ public class PowerUpRobot extends SprocketRobot { Sensitivity sensitivity; GyroLock lock; boolean manualLock; - CubeIntake intake; + public static CubeIntake intake; MainLift mainLift; // IntakeLift intakeLift; StateMachine autoClimb; From 8cd2886ea5765eebb32e85b77dca51a055c195f7 Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 21 Apr 2018 14:37:55 -0400 Subject: [PATCH 31/38] Teleop Runs --- src/main/java/frc/team555/robot/core/Hardware.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 8021782..57beda9 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -150,8 +150,8 @@ public static void init(){ intakeClosedSwitch = new DigitalInput(6); intakeOpenSwitch = new DigitalInput(8); - intakeUpperBound=new DigitalInput(5); - intakeLowerBound=new DigitalInput(9); + intakeUpperBound=new DigitalInput(7); + intakeLowerBound=new DigitalInput(10); //double ticksPerInch=6544.0/143.0;` //old/new=17.1859 * 1.25/(6544.0/143.0) From 130b52f17c0fbc20d41e7fa18398cad6cedd733e Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Thu, 26 Apr 2018 09:04:37 -0400 Subject: [PATCH 32/38] Before we fix the auto --- .../team555/robot/auto/AutoClimbSequence.java | 2 +- .../frc/team555/robot/auto/AutoWillTest.java | 11 +++++++ .../team555/robot/auto/SimpleDropCube.java | 2 +- .../team555/robot/components/CubeIntake.java | 33 ++++++++++++++----- .../team555/robot/components/IntakeLift.java | 7 ++-- .../team555/robot/components/MainLift.java | 2 +- .../java/frc/team555/robot/core/Hardware.java | 16 +++++---- .../frc/team555/robot/core/PowerUpRobot.java | 5 ++- 8 files changed, 55 insertions(+), 23 deletions(-) create mode 100644 src/main/java/frc/team555/robot/auto/AutoWillTest.java diff --git a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java index add250f..d92bcd9 100644 --- a/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java +++ b/src/main/java/frc/team555/robot/auto/AutoClimbSequence.java @@ -12,7 +12,7 @@ import org.montclairrobotics.sprocket.states.StateMachine; import static frc.team555.robot.components.MainLift.TOP; -@Deprecated + public class AutoClimbSequence extends StateMachine { private Lift lift; 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..9ddbdb0 --- /dev/null +++ b/src/main/java/frc/team555/robot/auto/AutoWillTest.java @@ -0,0 +1,11 @@ +package frc.team555.robot.auto; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.states.State; + +public class AutoWillTest extends AutoMode { + + public AutoWillTest(String name, State... states) { + super(name, states); + } +} diff --git a/src/main/java/frc/team555/robot/auto/SimpleDropCube.java b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java index 090fe34..46387f3 100644 --- a/src/main/java/frc/team555/robot/auto/SimpleDropCube.java +++ b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java @@ -27,7 +27,7 @@ public SimpleDropCube(MainLift mainLift, CubeIntake intake, GyroCorrection corre new AutoState() { @Override public void stateUpdate() { - intake.auto(); + intake.setAutoPower(.3); } @Override diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 8f8e07c..d6c0091 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -22,7 +22,8 @@ public class CubeIntake implements Updatable, Togglable{ - private double rotatePower=.1; + private double rotatePowerUp=.5; + private double rotatePowerDown=-.2; private boolean auto; @@ -43,9 +44,9 @@ public CubeIntake() { this.right = new Motor(Hardware.motorIntakeR); right.setInverted(true); //this.clamp = new Motor(Hardware.motorIntakeClamp); - LimitedMotor rotateMotor=new LimitedMotor(Hardware.motorIntakeRotate,true,Hardware.intakeLowerBound,Hardware.intakeUpperBound); - //Motor motor=new Motor(Hardware.motorIntakeRotate); - this.rotate=new TargetMotor(Hardware.intakeRotationEncoder,new BangBang(tolerance,rotatePower),rotateMotor); + //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() { @@ -59,7 +60,7 @@ public Vector get() { double y=-Control.auxStick.getY(); if(Math.abs(y)<0.1) { - y=-.05; + y=-0.1; } return new XY(x,y); } @@ -67,7 +68,7 @@ public Vector get() { - Control.intakeRotateDown.setPressAction(new ButtonAction() { + /*Control.intakeRotateDown.setPressAction(new ButtonAction() { @Override public void onAction() { rotate.setTarget(downPos); @@ -88,13 +89,14 @@ public void onAction() { rotate.setTarget(middlePos); } }); + */ Control.intakeRotateUpManual.setPressAction(new ButtonAction() { @Override public void onAction() { - rotate.setPower(rotatePower); + setPower(rotatePowerUp); } }); Control.intakeRotateUpManual.setReleaseAction(new ButtonAction() { @@ -107,7 +109,7 @@ public void onAction() { Control.intakeRotateDownManual.setPressAction(new ButtonAction() { @Override public void onAction() { - rotate.setPower(-rotatePower); + setPower(rotatePowerDown); } }); Control.intakeRotateDownManual.setReleaseAction(new ButtonAction() { @@ -122,6 +124,18 @@ public void onAction() { } + 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(); @@ -132,7 +146,8 @@ public void update() { } auto=false; Debug.msg("RotateMotor PID OUT",rotate.pid.get()); - Debug.msg("RotateMotor PID TGT",rotate.pid.getTarget()); + Debug.msg("RotateMotor PID TGT",rotate.pid.getTarget()); + Debug.msg("RotateMotor Encoder",rotate.pid.getCurInput()); } diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 1db9719..e3f253a 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -23,7 +23,8 @@ public class IntakeLift implements Lift{ private boolean auto=false; - public double MANUAL_POWER=.5; + public double MANUAL_POWER_UP=.5; + public double MANUAL_POWER_DOWN=-.3; public final double[] positions = {0D, 49-15}; // Todo: test values private int pos; private TargetMotor motors; @@ -79,7 +80,7 @@ public void onAction() { Control.intakeLiftManualUp.setPressAction(new ButtonAction() { @Override public void onAction() { - motors.setPower(MANUAL_POWER); + motors.setPower(MANUAL_POWER_UP); } }); @@ -87,7 +88,7 @@ public void onAction() { Control.intakeLiftManualDown.setPressAction(new ButtonAction() { @Override public void onAction() { - motors.setPower(-MANUAL_POWER); + motors.setPower(MANUAL_POWER_DOWN); } }); diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index dcd1658..da0d659 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -26,7 +26,7 @@ public class MainLift extends TargetMotor implements Lift { private final double speed = 1.0; public static final double TOP= 30834.0; - private final boolean LIMIT_SWITCH_DISABLED=true; + private final boolean LIMIT_SWITCH_DISABLED=false; private boolean auto=false; diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 57beda9..09a9706 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -58,6 +58,7 @@ private static class DeviceID { //public static final int motorIntakeClamp = 6; + //TODO: Check public static final int motorIntakeRotate = 6; public static final int motorIntakeL = 10; @@ -101,8 +102,8 @@ private static class DeviceID { public static WPI_TalonSRX motorIntakeRotate; - public static DigitalInput intakeClosedSwitch; - public static DigitalInput intakeOpenSwitch; + //public static DigitalInput intakeClosedSwitch; + //public static DigitalInput intakeOpenSwitch; public static DigitalInput intakeUpperBound; public static DigitalInput intakeLowerBound; @@ -136,6 +137,7 @@ public static void init(){ motorLiftMainFront = new WPI_TalonSRX(DeviceID.motorMainLiftFront); motorLiftMainBack = new WPI_TalonSRX(DeviceID.motorMainLiftBack); motorIntakeRotate = new WPI_TalonSRX(DeviceID.motorIntakeRotate); +// motorIntakeRotate.set motorIntakeR = new WPI_TalonSRX(DeviceID.motorIntakeR); motorIntakeL = new WPI_TalonSRX(DeviceID.motorIntakeL); @@ -147,11 +149,11 @@ public static void init(){ //motorIntakeClamp = new WPI_TalonSRX(DeviceID.motorIntakeClamp); - intakeClosedSwitch = new DigitalInput(6); - intakeOpenSwitch = new DigitalInput(8); + //intakeClosedSwitch = new DigitalInput(6); + //intakeOpenSwitch = new DigitalInput(8); intakeUpperBound=new DigitalInput(7); - intakeLowerBound=new DigitalInput(10); + intakeLowerBound=new DigitalInput(6); //double ticksPerInch=6544.0/143.0;` //old/new=17.1859 * 1.25/(6544.0/143.0) @@ -176,9 +178,9 @@ public static void init(){ //15.5 to 49.5 - liftEncoder = new SEncoder(new Encoder(4,5), 1/*31600/(82 - 54)*/); // todo: REALLY NEED TO BE SET + liftEncoder = new SEncoder(new Encoder(4,5), 1/*31600/(82 - 54)*/); intakeLiftEncoder = new TalonEncoder(motorLiftIntake, 1 /*1333600/(49.5 - 15.5)*/); - intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // todo, needs to be set + intakeRotationEncoder = new TalonEncoder(motorIntakeRotate, 1); // TODO: needs to be set diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 33aba83..12ea6fe 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team555.robot.auto.*; import frc.team555.robot.components.CubeIntake; +import frc.team555.robot.components.IntakeLift; import frc.team555.robot.components.MainLift; import frc.team555.robot.utils.CoastMotor; import frc.team555.robot.utils.Side; @@ -40,7 +41,7 @@ public class PowerUpRobot extends SprocketRobot { boolean manualLock; public static CubeIntake intake; MainLift mainLift; - // IntakeLift intakeLift; + IntakeLift intakeLift; StateMachine autoClimb; //SendableChooser startSideChooser; @@ -82,6 +83,7 @@ public Boolean get() { 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); @@ -341,6 +343,7 @@ public void onAction() { 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(); From b2fbcf8a3b5315061d6cda414679355bdbeefd8c Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 28 Apr 2018 10:50:13 -0400 Subject: [PATCH 33/38] Final Worlds code --- .../frc/team555/robot/auto/AutoSwitch.java | 7 ++- .../frc/team555/robot/auto/AutoSwitchLR.java | 22 ++++---- .../frc/team555/robot/auto/AutoSwitchMid.java | 14 ++--- .../frc/team555/robot/auto/AutoWillTest.java | 45 +++++++++++---- .../team555/robot/auto/SimpleDropCube.java | 2 +- .../frc/team555/robot/auto/TopCubeAuto.java | 2 +- .../team555/robot/components/CubeIntake.java | 2 +- .../team555/robot/components/IntakeLift.java | 4 +- .../team555/robot/components/MainLift.java | 56 +++++++++++++------ .../java/frc/team555/robot/core/Control.java | 4 +- .../java/frc/team555/robot/core/Hardware.java | 2 + .../frc/team555/robot/core/PowerUpRobot.java | 9 +-- 12 files changed, 109 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/team555/robot/auto/AutoSwitch.java b/src/main/java/frc/team555/robot/auto/AutoSwitch.java index 2a9236f..883d766 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitch.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitch.java @@ -3,11 +3,11 @@ 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; + /*protected static double POWER = 0.75; - public AutoSwitch(String name, State... states) { + // public AutoSwitch(String name, State... states) { super(name, states); } @@ -19,3 +19,4 @@ 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 index c8a37e7..666a847 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchLR.java @@ -6,28 +6,28 @@ 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 */ + /* Reset gyro new ResetGyro(PowerUpRobot.correction), - /* Drive forward 12 inches */ + /* Drive forward 12 inches new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction), - /* Turn π/2 radians (toward the nearest wall) */ + /* 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 */ + /* Drive forward 36 inches new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), - /* Turn π/2 radians (in the opposite direction) */ + /* 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 */ + /* Drive forward 36 inches new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), - /* Turn π/2 radians (in the opposite direction) */ + /* 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 */ + /* 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 */ + /* 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 index da82d9c..3334ff6 100644 --- a/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java +++ b/src/main/java/frc/team555/robot/auto/AutoSwitchMid.java @@ -6,21 +6,21 @@ 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 */ + /* Reset gyro new ResetGyro(PowerUpRobot.correction), - /* Drive forward 12 inches */ + /* Drive forward 12 inches new DriveEncoderGyro(12, POWER, Angle.ZERO, true, PowerUpRobot.correction), - /* Turn π/2 radians (in the correct directon) */ + /* 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 */ + /* Drive forward 36 inches new DriveEncoderGyro(36, POWER, Angle.ZERO, true, PowerUpRobot.correction), - /* Move the lift, turn, and drop the cube into the switch */ + /* 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 index 9ddbdb0..6b675ba 100644 --- a/src/main/java/frc/team555/robot/auto/AutoWillTest.java +++ b/src/main/java/frc/team555/robot/auto/AutoWillTest.java @@ -1,11 +1,34 @@ -package frc.team555.robot.auto; - -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.states.State; - -public class AutoWillTest extends AutoMode { - - public AutoWillTest(String name, State... states) { - super(name, states); - } -} +//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/SimpleDropCube.java b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java index 46387f3..ab1a56e 100644 --- a/src/main/java/frc/team555/robot/auto/SimpleDropCube.java +++ b/src/main/java/frc/team555/robot/auto/SimpleDropCube.java @@ -18,7 +18,7 @@ public class SimpleDropCube extends StateMachine{ public SimpleDropCube(MainLift mainLift, CubeIntake intake, GyroCorrection correction, Input side){ super( new MultiState( - new MoveLift(mainLift,MainLift.TOP*0.6,1,true), + new MoveLift(mainLift,MainLift.TOP*0.5,1,true), new DriveEncoderGyro(140, .7, Angle.ZERO, false, correction) ), new SideTurn(correction, false, side), diff --git a/src/main/java/frc/team555/robot/auto/TopCubeAuto.java b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java index f728074..da879bc 100644 --- a/src/main/java/frc/team555/robot/auto/TopCubeAuto.java +++ b/src/main/java/frc/team555/robot/auto/TopCubeAuto.java @@ -23,7 +23,7 @@ public Side get() { return SwitchAuto.startSidesChooser.getSelected(); } }), SwitchAuto.startSide), - new ConditionalState(new DriveEncoderGyro(180,0.5,Angle.ZERO,false,correction),new Input(){ + 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 d6c0091..8c12bc5 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -60,7 +60,7 @@ public Vector get() { double y=-Control.auxStick.getY(); if(Math.abs(y)<0.1) { - y=-0.1; + y=-0.05; } return new XY(x,y); } diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index e3f253a..18497d8 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -23,8 +23,8 @@ public class IntakeLift implements Lift{ private boolean auto=false; - public double MANUAL_POWER_UP=.5; - public double MANUAL_POWER_DOWN=-.3; + public double MANUAL_POWER_UP=1; + public double MANUAL_POWER_DOWN=-.4; public final double[] positions = {0D, 49-15}; // Todo: test values private int pos; private TargetMotor motors; diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index da0d659..d59f161 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -41,22 +41,22 @@ public MainLift(){ mode = Mode.POWER; // Manual Up - Control.mainLiftManualUp.setPressAction(new ButtonAction() { - @Override - public void onAction() { - set(speed); - } - }); - - Control.mainLiftManualUp.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - set(0); - } - }); - - // Manual Down - Control.mainLiftManualDown.setHeldAction(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() { if(LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) { @@ -75,7 +75,7 @@ public void onAction() { - Control.mainLiftManualDown.setReleaseAction(new ButtonAction() { + Control.mainLiftAutoDown.setReleaseAction(new ButtonAction() { @Override public void onAction() { set(0); @@ -106,6 +106,28 @@ public void onAction() { } } }); + Control.mainLiftManualUp.setHeldAction(new ButtonAction() { + @Override + public void onAction() { + if(Hardware.liftEncoder.getInches().get()<31296.0) + { + Hardware.motorLiftMainFront.set(.5); + Hardware.motorLiftMainBack.set(.5); + } + else + { + Hardware.motorLiftMainFront.set(0); + Hardware.motorLiftMainBack.set(0); + } + } + }); + Control.mainLiftManualUp.setReleaseAction(new ButtonAction() { + @Override + public void onAction() { + Hardware.motorLiftMainFront.set(0); + Hardware.motorLiftMainBack.set(0); + } + }); Control.mainLiftAutoUp.setReleaseAction(new ButtonAction() { @Override diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 8153f49..d5ddfaf 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -28,7 +28,7 @@ public class Control { //intakeLiftBottom, mainLiftAutoUp, mainLiftManualUp, - mainLiftManualDown, + mainLiftAutoDown, // mainLiftTop, // mainLiftBottom, @@ -88,7 +88,7 @@ public static void init() { mainLiftAutoUp = new JoystickButton(auxStick, 1); */ mainLiftManualUp = new JoystickButton(auxStick,4); - mainLiftManualDown = new JoystickButton(auxStick,3); + mainLiftAutoDown = new JoystickButton(auxStick,3); intakeLiftManualUp = new JoystickButton(auxStick,6); intakeLiftManualDown = new JoystickButton(auxStick,5); mainLiftAutoUp = new JoystickButton(auxStick, 1); diff --git a/src/main/java/frc/team555/robot/core/Hardware.java b/src/main/java/frc/team555/robot/core/Hardware.java index 09a9706..71a00ff 100644 --- a/src/main/java/frc/team555/robot/core/Hardware.java +++ b/src/main/java/frc/team555/robot/core/Hardware.java @@ -13,6 +13,7 @@ //15.5 to 49.5 +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.*; import frc.team555.robot.utils.NavXInput; @@ -134,6 +135,7 @@ public static void init(){ motorDriveFR.setInverted(true); motorLiftIntake = new WPI_TalonSRX(DeviceID.motorIntakeLift); + motorLiftIntake.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0); motorLiftMainFront = new WPI_TalonSRX(DeviceID.motorMainLiftFront); motorLiftMainBack = new WPI_TalonSRX(DeviceID.motorMainLiftBack); motorIntakeRotate = new WPI_TalonSRX(DeviceID.motorIntakeRotate); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 12ea6fe..3108e3a 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -336,9 +336,9 @@ public void onAction() { /* Joshua Rapoport: AutoSwitch (left, right, middle) */ - addAutoMode(AutoSwitch.fromSide(Side.LEFT)); - addAutoMode(AutoSwitch.fromSide(Side.RIGHT)); - addAutoMode(AutoSwitch.fromMiddle()); + // 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); @@ -412,6 +412,7 @@ public void onAction() { } }); visionThread.start();*/ + } @Override @@ -443,7 +444,7 @@ public void update(){ 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",in.getInches().get()); + SmartDashboard.putNumber("Intake Lift Encoder",Hardware.motorLiftIntake.getSensorCollection().getQuadraturePosition()); gyroLocking(); //startSide = startSideChooser.getSelected(); } From d957f8b7b1e6599e4346b6b16768de5140299923 Mon Sep 17 00:00:00 2001 From: Jack Hymowitz Date: Tue, 1 May 2018 22:23:06 -0400 Subject: [PATCH 34/38] Added limit switches to Target Motor (should have been here all along, also done with web editor so code likely doesn't compile) --- .../frc/team555/robot/utils/TargetMotor.java | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team555/robot/utils/TargetMotor.java b/src/main/java/frc/team555/robot/utils/TargetMotor.java index bfaf839..16992a3 100644 --- a/src/main/java/frc/team555/robot/utils/TargetMotor.java +++ b/src/main/java/frc/team555/robot/utils/TargetMotor.java @@ -18,7 +18,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 +61,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 +75,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 +87,21 @@ 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.get() || pow<0 && lowerBound.get()) + { + pow=0; } + super.set(pow); } public double getTarget(){ From 8cd1b8972d3ca6d70fef90e9ad5b81cb89d04595 Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Tue, 8 May 2018 15:12:51 -0400 Subject: [PATCH 35/38] Updates to make it "Safer" for Petting Zoo; Revert after --- .../team555/robot/components/CubeIntake.java | 5 +++-- .../team555/robot/components/IntakeLift.java | 6 +++-- .../team555/robot/components/MainLift.java | 22 ++++++++++++------- .../java/frc/team555/robot/core/Control.java | 4 ++-- .../frc/team555/robot/core/PowerUpRobot.java | 9 ++++---- 5 files changed, 28 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 8c12bc5..486aa9d 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -36,6 +36,7 @@ public class CubeIntake implements Updatable, Togglable{ public final int downPos = 2; public final int middlePos = 1; + private double speed=0.5; public Input power; @@ -141,8 +142,8 @@ public void update() { Vector p = power.get(); if(!auto) { - left.set(p.getY() - p.getX()*.25); - right.set(p.getY() + p.getX()*.25); + 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()); diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 18497d8..85da594 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -77,7 +77,9 @@ public void onAction() { */ //Start the motors in manual mode //IntakeLift up (manual backup control) - Control.intakeLiftManualUp.setPressAction(new ButtonAction() { + + //COMMENT OUT FOR PETTING ZOO + /*Control.intakeLiftManualUp.setPressAction(new ButtonAction() { @Override public void onAction() { motors.setPower(MANUAL_POWER_UP); @@ -91,7 +93,7 @@ public void onAction() { motors.setPower(MANUAL_POWER_DOWN); } - }); + });*/ //Stop the motors in manual mode ButtonAction stop=new ButtonAction() { diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index d59f161..95f4ebd 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -24,19 +24,21 @@ public class MainLift extends TargetMotor implements Lift { - private final double speed = 1.0; + private final double speed = 0.5; public static final double TOP= 30834.0; private final boolean LIMIT_SWITCH_DISABLED=false; private boolean auto=false; + public boolean hasReset=false; + private int upPosition; private int downPosition; private DigitalInput bottomLimitSwitch; public MainLift(){ - super(Hardware.liftEncoder, new BangBang(1,1), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); + super(Hardware.liftEncoder, new BangBang(1,.5), new Motor(Hardware.motorLiftMainFront), new Motor(Hardware.motorLiftMainBack)); mode = Mode.POWER; @@ -59,7 +61,8 @@ public MainLift(){ Control.mainLiftAutoDown.setHeldAction(new ButtonAction() { @Override public void onAction() { - if(LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) { + if((LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) + &&Control.auxStick.getRawButton(1)) { if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); } @@ -69,6 +72,7 @@ public void onAction() { }else{ set(0); encoder.reset(); + hasReset=true; } } }); @@ -91,7 +95,7 @@ public void onAction() { } } });*/ - Control.mainLiftAutoUp.setHeldAction(new ButtonAction() { + /*Control.mainLiftAutoUp.setHeldAction(new ButtonAction() { @Override public void onAction() { if(Hardware.liftEncoder.getInches().get()100) ArrayList> steps = new ArrayList<>(); - sensitivity=new Sensitivity(1,0.6); + sensitivity=new Sensitivity(fullSpeed,0.6); lock = new GyroLock(correction); steps.add(new Deadzone()); steps.add(sensitivity); @@ -464,11 +465,11 @@ private void gyroLocking(){ //lock.update(); if(Hardware.liftEncoder.getInches().get()>MainLift.TOP*0.5&&!Control.driveStick.getRawButton(3)||Control.driveStick.getRawButton(2)) { - sensitivity.set(0.4,0.3); + sensitivity.set(halfSpeed,0.3); } else { - sensitivity.set(1,0.6); + sensitivity.set(fullSpeed,0.6); } } From d2ffb5fb5dafe8245bcf5e37de8fb2a92356fd21 Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 12 May 2018 13:55:17 -0400 Subject: [PATCH 36/38] Update before petting zoo --- .../frc/team555/robot/components/MainLift.java | 14 +++++++++----- .../java/frc/team555/robot/utils/TargetMotor.java | 5 ++++- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 95f4ebd..2bcef4e 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -24,7 +24,7 @@ public class MainLift extends TargetMotor implements Lift { - private final double speed = 0.5; + private final double speed =1; public static final double TOP= 30834.0; private final boolean LIMIT_SWITCH_DISABLED=false; @@ -62,7 +62,8 @@ public MainLift(){ @Override public void onAction() { if((LIMIT_SWITCH_DISABLED || !Hardware.liftLimitSwitch.get()) - &&Control.auxStick.getRawButton(1)) { + //&&Control.auxStick.getRawButton(1) + ) { if(encoder.getInches().get()>TOP*0.2||Control.auxStick.getRawButton(7)) { set(-speed); } @@ -114,11 +115,11 @@ public void onAction() { @Override public void onAction() { if(Hardware.liftEncoder.getInches().get()<31296.0 - &&Control.auxStick.getRawButton(1) + //&&Control.auxStick.getRawButton(1) &&hasReset) { - Hardware.motorLiftMainFront.set(.5); - Hardware.motorLiftMainBack.set(.5); + Hardware.motorLiftMainFront.set(1); + Hardware.motorLiftMainBack.set(1); } else { @@ -143,6 +144,9 @@ public void onAction() { } });*/ setPower(0); + + + // Lift Bottom diff --git a/src/main/java/frc/team555/robot/utils/TargetMotor.java b/src/main/java/frc/team555/robot/utils/TargetMotor.java index 16992a3..573cd84 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; @@ -97,10 +99,11 @@ public void update() { public void safeSet(double pow) { - if(pow>0 && upperBound.get() || pow<0 && lowerBound.get()) + if(pow>0 &&(upperBound!=null && upperBound.get()) || pow<0 && (lowerBound!=null && lowerBound.get())) { pow=0; } + Debug.msg("Setting a power",pow); super.set(pow); } From eb4901f013f3573aafd1c551c62d08983eae6e0a Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Sat, 12 May 2018 14:11:23 -0400 Subject: [PATCH 37/38] Final petting zoo code... --- .../java/frc/team555/robot/components/MainLift.java | 2 +- src/main/java/frc/team555/robot/core/Control.java | 11 ++++++----- .../java/frc/team555/robot/core/PowerUpRobot.java | 4 ++-- .../java/frc/team555/robot/utils/TargetMotor.java | 6 +++--- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/team555/robot/components/MainLift.java b/src/main/java/frc/team555/robot/components/MainLift.java index 2bcef4e..18e58d5 100644 --- a/src/main/java/frc/team555/robot/components/MainLift.java +++ b/src/main/java/frc/team555/robot/components/MainLift.java @@ -38,7 +38,7 @@ public class MainLift extends TargetMotor implements Lift { public MainLift(){ - super(Hardware.liftEncoder, new BangBang(1,.5), 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; diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 679f6ce..04bec5c 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -38,9 +38,10 @@ public class Control { intakeRotateDown, intakeRotateMiddle, intakeRotateUpManual, - intakeRotateDownManual, + intakeRotateDownManual //intakeSubroutine; - autoClimb; + //autoClimb + ; //public static int FieldCentricID=2; @@ -87,13 +88,13 @@ public static void init() { intakeLiftManualDown = new JoystickButton(auxStick,4); mainLiftAutoUp = new JoystickButton(auxStick, 1); */ - mainLiftManualUp = new JoystickButton(auxStick,4); - mainLiftAutoDown = new JoystickButton(auxStick,3); + mainLiftManualUp = new JoystickButton(auxStick,3); + mainLiftAutoDown = new JoystickButton(auxStick,2); intakeLiftManualUp = new JoystickButton(auxStick,6); intakeLiftManualDown = new JoystickButton(auxStick,5); //mainLiftAutoUp = new JoystickButton(auxStick, 1); halfSpeed=new JoystickButton(driveStick,2); - autoClimb = new JoystickButton(auxStick, 2); + //autoClimb = new JoystickButton(auxStick, 2); // intakeLiftTop = new JoystickButton(auxStick, 12); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 06aac4e..1864216 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -377,7 +377,7 @@ public void onAction() { - +/* Control.autoClimb.setPressAction(new ButtonAction() { @Override public void onAction() { @@ -398,7 +398,7 @@ public void onAction() { autoClimb.stop(); } }); - +*/ // vision stuff // CameraServer.getInstance().startAutomaticCapture(); /*UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); diff --git a/src/main/java/frc/team555/robot/utils/TargetMotor.java b/src/main/java/frc/team555/robot/utils/TargetMotor.java index 573cd84..8be8d0e 100644 --- a/src/main/java/frc/team555/robot/utils/TargetMotor.java +++ b/src/main/java/frc/team555/robot/utils/TargetMotor.java @@ -93,16 +93,16 @@ public void update() { } else { - safeSet(power); + //safeSet(power); } } public void safeSet(double pow) { - if(pow>0 &&(upperBound!=null && upperBound.get()) || pow<0 && (lowerBound!=null && lowerBound.get())) + /*if(pow>0 &&(upperBound!=null && upperBound.get()) || pow<0 && (lowerBound!=null && lowerBound.get())) { pow=0; - } + }*/ Debug.msg("Setting a power",pow); super.set(pow); } From d6ed2c10967583a762c751894ca49a7334bfc948 Mon Sep 17 00:00:00 2001 From: Asus-Zenbook Date: Thu, 17 May 2018 19:26:09 -0400 Subject: [PATCH 38/38] power down after library --- .../java/frc/team555/robot/components/CubeIntake.java | 2 +- .../java/frc/team555/robot/components/IntakeLift.java | 4 ++-- src/main/java/frc/team555/robot/core/Control.java | 4 ++-- .../java/frc/team555/robot/core/PowerUpRobot.java | 11 ++++++----- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/team555/robot/components/CubeIntake.java b/src/main/java/frc/team555/robot/components/CubeIntake.java index 486aa9d..94a0a50 100644 --- a/src/main/java/frc/team555/robot/components/CubeIntake.java +++ b/src/main/java/frc/team555/robot/components/CubeIntake.java @@ -36,7 +36,7 @@ public class CubeIntake implements Updatable, Togglable{ public final int downPos = 2; public final int middlePos = 1; - private double speed=0.5; + private double speed=1; public Input power; diff --git a/src/main/java/frc/team555/robot/components/IntakeLift.java b/src/main/java/frc/team555/robot/components/IntakeLift.java index 85da594..6b96a11 100644 --- a/src/main/java/frc/team555/robot/components/IntakeLift.java +++ b/src/main/java/frc/team555/robot/components/IntakeLift.java @@ -79,7 +79,7 @@ public void onAction() { //IntakeLift up (manual backup control) //COMMENT OUT FOR PETTING ZOO - /*Control.intakeLiftManualUp.setPressAction(new ButtonAction() { + Control.intakeLiftManualUp.setPressAction(new ButtonAction() { @Override public void onAction() { motors.setPower(MANUAL_POWER_UP); @@ -93,7 +93,7 @@ public void onAction() { motors.setPower(MANUAL_POWER_DOWN); } - });*/ + }); //Stop the motors in manual mode ButtonAction stop=new ButtonAction() { diff --git a/src/main/java/frc/team555/robot/core/Control.java b/src/main/java/frc/team555/robot/core/Control.java index 04bec5c..6152869 100644 --- a/src/main/java/frc/team555/robot/core/Control.java +++ b/src/main/java/frc/team555/robot/core/Control.java @@ -88,8 +88,8 @@ public static void init() { intakeLiftManualDown = new JoystickButton(auxStick,4); mainLiftAutoUp = new JoystickButton(auxStick, 1); */ - mainLiftManualUp = new JoystickButton(auxStick,3); - mainLiftAutoDown = new JoystickButton(auxStick,2); + mainLiftManualUp = new JoystickButton(auxStick,4); + mainLiftAutoDown = new JoystickButton(auxStick,3); intakeLiftManualUp = new JoystickButton(auxStick,6); intakeLiftManualDown = new JoystickButton(auxStick,5); //mainLiftAutoUp = new JoystickButton(auxStick, 1); diff --git a/src/main/java/frc/team555/robot/core/PowerUpRobot.java b/src/main/java/frc/team555/robot/core/PowerUpRobot.java index 1864216..bbb81d7 100644 --- a/src/main/java/frc/team555/robot/core/PowerUpRobot.java +++ b/src/main/java/frc/team555/robot/core/PowerUpRobot.java @@ -50,8 +50,8 @@ public class PowerUpRobot extends SprocketRobot { private static final int IMG_WIDTH = 320; private static final int IMG_HEIGHT = 240; - private static final double fullSpeed=0.5; - private static final double halfSpeed=0.4; + 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); @@ -184,7 +184,7 @@ else if(x>100) ArrayList> steps = new ArrayList<>(); - sensitivity=new Sensitivity(fullSpeed,0.6); + sensitivity=new Sensitivity(fullSpeed,fullSpeed*0.8); lock = new GyroLock(correction); steps.add(new Deadzone()); steps.add(sensitivity); @@ -231,6 +231,7 @@ public void onAction() { } }); + Control.halfSpeed.setReleaseAction(new ButtonAction() { Control.halfSpeed.setReleaseAction(new ButtonAction() { @Override public void onAction() { @@ -465,11 +466,11 @@ private void gyroLocking(){ //lock.update(); if(Hardware.liftEncoder.getInches().get()>MainLift.TOP*0.5&&!Control.driveStick.getRawButton(3)||Control.driveStick.getRawButton(2)) { - sensitivity.set(halfSpeed,0.3); + sensitivity.set(halfSpeed,0.8*fullSpeed); } else { - sensitivity.set(fullSpeed,0.6); + sensitivity.set(fullSpeed,0.8*fullSpeed); } }