diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f0cd685..d6e8e17 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,40 +3,41 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be - * declared globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ public final class Constants { - public static final double hoodFromTrench = 600; - public static final double hoodTriangle = 800; - public static final double hoodAutoLine = 767; - public static final double turretFromTrench = 6; - public static final double turretClose = 0; - public static final double ticksPerRevolutionLow = 19; + public static final double hoodFromTrench = 930; + public static final double hoodTriangle = 550; + public static final double hoodAutoLine = 1075; + public static final double turretFromTrench = -6; + public static final double turretLine = -7; + public static final double turretTriangle = 0; + public static final double ticksPerRevolutionLow = 18.6; public static final double ticksPerRevolutionHigh = 6.4; public static final double ratioGearLow = 18.86; public static final double ratioGearHigh = 6.45; public static final double wheelCircumferenceMeters = 6*Math.PI*0.0254; + //HoodTriangle: 590 + //HoodLine: 1117 + //HoodTrench: + + //is that guys name Howard?? + // idk + //Seth's granpa + public static final class DriveConstants { // public static boolean reverse = true; public static boolean reverse = false; - public static final double kTrackWidth = 0.584; - // public static final double kTrackWidth = 0.5; + public static final double kTrackWidth = 0.584/1.5; + // public static final double kTrackWidth = 0.0000001; public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackWidth); - public static final double ksVoltsLow = 0.147; + public static final double ksVoltsLow = 0.174; // public static final double kvVoltSecondsPerMeterLow = 1.88; //estimate - public static final double kvVoltSecondsPerMeterLow = 4.47; //characterize - public static final double kaVoltSecondsSquaredPerMeterLow = 0.552; //0.1225 + public static final double kvVoltSecondsPerMeterLow = 4.74; //characterize + public static final double kaVoltSecondsSquaredPerMeterLow = 0.6; - public static final double kPDriveVelLow = 16.3; + public static final double kPDriveVelLow = 3; public static final double ksVoltsHigh = 0.122; public static final double kvVoltSecondsPerMeterHigh = 0.64; //estimate diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0e1b038..90ca4e7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,29 +1,26 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot; import java.io.IOException; - import com.revrobotics.CANSparkMax.IdleMode; - import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.HoodCommand; +import frc.robot.commands.PIDFlywheel; import frc.robot.commands.PIDHood; import frc.robot.commands.PIDTurret; +import frc.robot.commands.TurretCommand; +import frc.robot.commands.auto.AutoIntaking; +import frc.robot.commands.auto.BasicAuto; +import frc.robot.commands.auto.ThisIsDumb; import frc.robot.commands.auto.paths.BasicTurn; import frc.robot.commands.auto.paths.Chain; import frc.robot.commands.auto.paths.Example; import frc.robot.commands.auto.paths.Fiveball; -import frc.robot.commands.auto.paths.Forward; +import frc.robot.commands.auto.paths.Move; import frc.robot.commands.auto.paths.Secondary; import frc.robot.commands.auto.paths.Smiles; import frc.robot.commands.auto.paths.TurnReverse; @@ -31,12 +28,6 @@ import frc.robot.subsystems.IntakeSub; import frc.robot.subsystems.LidarSub; -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ public class Robot extends TimedRobot { private Command m_autonomousCommand; SendableChooser m_chooser; @@ -44,22 +35,27 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; public static PIDTurret pt = new PIDTurret(); + public static TurretCommand tc = new TurretCommand(); public static PIDHood ph = new PIDHood(Constants.hoodFromTrench); + public static HoodCommand hc = new HoodCommand(); + + public static boolean auto; - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ @Override public void robotInit() { m_robotContainer = new RobotContainer(); configRobot(); m_chooser = new SendableChooser<>(); - m_chooser.addOption("forward", new Forward(1)); + m_chooser.addOption("forward", new Move(4, false)); + m_chooser.addOption("reverse", new Move(1, true)); m_chooser.addOption("reverse turn", new TurnReverse()); m_chooser.addOption("basic turning", new BasicTurn()); try { + m_chooser.addOption("basic auto", new BasicAuto()); + m_chooser.addOption("Intake intakes", new AutoIntaking()); + m_chooser.addOption("Just Testing Fwd", new ThisIsDumb()); + m_chooser.addOption("example", new Example()); m_chooser.addOption("chain", new Chain()); m_chooser.addOption("smiles", new Smiles()); @@ -71,30 +67,16 @@ public void robotInit() { } catch (final IOException e1) { e1.printStackTrace(); } - SmartDashboard.putData("Auto choochooer", m_chooser); + SmartDashboard.putData("Auto chooBchooser", m_chooser); } - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. - */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); Smartdashboarding(); } - /** - * This function is called once each time the robot enters Disabled mode. - */ @Override public void disabledInit() { } @@ -103,46 +85,47 @@ public void disabledInit() { public void disabledPeriodic() { } - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ @Override public void autonomousInit() { m_autonomousCommand = m_chooser.getSelected(); + auto = true; + // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } } - /** - * This function is called periodically during autonomous. - */ @Override public void autonomousPeriodic() { } @Override public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. + auto = false; if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } } - /** - * This function is called periodically during operator control. - */ @Override public void teleopPeriodic() { + // System.out.println(getRequiredRPM(20)); + // RobotContainer.turretMotor.overrideLimitSwitchesEnable(false); // RobotContainer.turretMotor.overrideSoftLimitsEnable(false); shooter(); + // RobotContainer.turretMotor.set(RobotContainer.manipulator.getRawAxis(4)); + + if(RobotContainer.mbumperLeft.get()){ + RobotContainer.CPMshift.set(Value.kForward); + } + + if(RobotContainer.mbumperRight.get()){ + RobotContainer.CPMshift.set(Value.kReverse); + } if(RobotContainer.mback.get()){ RobotContainer.intakePistons.set(Value.kForward); @@ -152,11 +135,11 @@ public void teleopPeriodic() { RobotContainer.intakePistons.set(Value.kReverse); } - if(RobotContainer.manipulator.getPOV() == 90){ + if(RobotContainer.mterribleRight.get()){ RobotContainer.CPMshift.set(Value.kForward); } - if(RobotContainer.manipulator.getPOV() == 270){ + if(RobotContainer.mterribleLeft.get()){ RobotContainer.CPMshift.set(Value.kReverse); } @@ -173,32 +156,33 @@ public void testInit() { } - /** - * This function is called periodically during test mode. - */ @Override public void testPeriodic() { } + public static double getRequiredRPM(double vel){ + return (120 * vel) / (2 * Math.PI * RobotContainer.flywheelSub.flywheelRadius); + } + public void Smartdashboarding(){ // SmartDashboard.putNumber("Current Gear", RobotContainer.driveSub.getCurrentGear()); - // SmartDashboard.putString("Wheel Speeds", RobotContainer.driveSub.getWheelSpeeds().toString()); - // SmartDashboard.putNumber("l1",RobotContainer.l1.getEncoder().getPosition()); + SmartDashboard.putString("Wheel Speeds", RobotContainer.driveSub.getWheelSpeeds().toString()); + SmartDashboard.putNumber("l1",RobotContainer.l1.getEncoder().getPosition()); // SmartDashboard.putNumber("l2",RobotContainer.l2.getEncoder().getPosition()); - // SmartDashboard.putNumber("r1",-RobotContainer.r1.getEncoder().getPosition()); + SmartDashboard.putNumber("r1",-RobotContainer.r1.getEncoder().getPosition()); // SmartDashboard.putNumber("r2",-RobotContainer.r2.getEncoder().getPosition()); - // SmartDashboard.putNumber("Gyro Heading", RobotContainer.driveSub.getHeading()); - // SmartDashboard.putNumber("Raw gyro", RobotContainer.navX.getAngle()); - // SmartDashboard.putString("Pose", RobotContainer.driveSub.getPose().toString()); + SmartDashboard.putNumber("Gyro Heading", RobotContainer.driveSub.getHeading()); + SmartDashboard.putNumber("Raw gyro", RobotContainer.navX.getAngle()); + SmartDashboard.putString("Pose", RobotContainer.driveSub.getPose().toString()); // SmartDashboard.putString("Pose Rot", RobotContainer.driveSub.getPose().getRotation().toString()); // SmartDashboard.putString("Pose Dist", RobotContainer.driveSub.getPose().getTranslation().toString()); // SmartDashboard.putNumber("Average encoder distance", RobotContainer.driveSub.getAverageEncoderDistance()); // if(RobotContainer.dsL.get() == Value.kForward){ SmartDashboard.putNumber("R Dist", (-RobotContainer.r1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); - // SmartDashboard.putNumber("L Dist", (RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); + SmartDashboard.putNumber("L Dist", (RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); SmartDashboard.putNumber("R Vel", (-RobotContainer.r1.getEncoder().getVelocity()/60 / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); - // SmartDashboard.putNumber("L Vel", (RobotContainer.l1.getEncoder().getVelocity()/60 / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); + SmartDashboard.putNumber("L Vel", (RobotContainer.l1.getEncoder().getVelocity()/60 / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); // }else{ // SmartDashboard.putNumber("R Dist", (-RobotContainer.r1.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters); // SmartDashboard.putNumber("L Dist", (RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters); @@ -207,17 +191,12 @@ public void Smartdashboarding(){ // } SmartDashboard.putNumber("R Dist", (RobotContainer.r2.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); SmartDashboard.putNumber("R Vel", (RobotContainer.r2.getEncoder().getVelocity()/60 / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); - - // SmartDashboard.putNumber("axis 1", RobotContainer.driver.getRawAxis(1)); - // SmartDashboard.putNumber("axis 4", RobotContainer.driver.getRawAxis(4)); - // SmartDashboard.putString("Sol", RobotContainer.dsL.get().toString()); + SmartDashboard.putNumber("Rotation speeds", RobotContainer.driveSub.getRotationSpeed()); + SmartDashboard.putNumber("Wheel vels", RobotContainer.driveSub.getWheelVelocity()); + SmartDashboard.putNumber("hood position", RobotContainer.hoodMotor.getSensorCollection().getQuadraturePosition()); SmartDashboard.putNumber("turret position", RobotContainer.turretMotor.getSensorCollection().getQuadraturePosition()); - SmartDashboard.putNumber("lift position", RobotContainer.lift.getSensorCollection().getQuadraturePosition()); - SmartDashboard.putNumber("winch position 1", RobotContainer.winch1.getSensorCollection().getQuadraturePosition()); - SmartDashboard.putNumber("winch position 2", RobotContainer.winch1.getSensorCollection().getQuadraturePosition()); - - SmartDashboard.putNumber("mani POV", RobotContainer.manipulator.getPOV()); + // SmartDashboard.putNumber("lift position", RobotContainer.lift.getSensorCollection().getQuadraturePosition()); SmartDashboard.putNumber("LiDAR dist", RobotContainer.lidarSub.getDistance()); // SmartDashboard.putString("Color L", RobotContainer.cpmSub.getColorL()); @@ -225,7 +204,7 @@ public void Smartdashboarding(){ SmartDashboard.putBoolean("Climb lock", RobotContainer.climbLock.get() == Value.kReverse); SmartDashboard.putBoolean("Winch pull", RobotContainer.winchShift.get() == Value.kReverse); - SmartDashboard.putBoolean("quad pos", RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14000); + // SmartDashboard.putBoolean("quad pos", RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14000); // SmartDashboard.putNumber("Rotation Speed of Wheel", RobotContainer.driveSub.getRotationSpeed(RobotContainer.driveSub.getCurrentGear())); // SmartDashboard.putNumber("RPM limit gear one", DriveSub.limitRotSpdGear1); // SmartDashboard.putNumber("FtPerSecondOfRobot", RobotContainer.driveSub.getRotationSpeed(RobotContainer.driveSub.getCurrentGear()) * (DriveSub.wheelDiameter * Math.PI) / 60); @@ -249,31 +228,29 @@ public void Smartdashboarding(){ public void configRobot(){ RobotContainer.diffDrive.setSafetyEnabled(false); - RobotContainer.dsL.set(Value.kForward); + RobotContainer.dsL.set(Value.kReverse); RobotContainer.intake1.enableCurrentLimit(true); RobotContainer.intake1.configPeakCurrentDuration(0,0); - RobotContainer.intake1.configPeakCurrentLimit(30,0); + RobotContainer.intake1.configPeakCurrentLimit(50,0); - RobotContainer.intake2.enableCurrentLimit(true); - RobotContainer.intake2.configPeakCurrentDuration(0,0); - RobotContainer.intake2.configPeakCurrentLimit(30,0); + // RobotContainer.intake2.enableCurrentLimit(true); + // RobotContainer.intake2.configPeakCurrentDuration(0,0); + // RobotContainer.intake2.configPeakCurrentLimit(50,0); - RobotContainer.intake3.enableCurrentLimit(true); - RobotContainer.intake3.configPeakCurrentDuration(0,0); - RobotContainer.intake3.configPeakCurrentLimit(30,0); + // RobotContainer.intake3.enableCurrentLimit(true); + // RobotContainer.intake3.configPeakCurrentDuration(0,0); + // RobotContainer.intake3.configPeakCurrentLimit(30,0); - RobotContainer.r1.setInverted(true); - RobotContainer.r2.setInverted(true); RobotContainer.r1.setIdleMode(IdleMode.kBrake); RobotContainer.r2.setIdleMode(IdleMode.kBrake); RobotContainer.l1.setIdleMode(IdleMode.kBrake); RobotContainer.l2.setIdleMode(IdleMode.kBrake); RobotContainer.flywheelMotor.setIdleMode(IdleMode.kCoast); RobotContainer.flywheelMotor2.setIdleMode(IdleMode.kCoast); - RobotContainer.l1.setInverted(true); - RobotContainer.l2.setInverted(true); final double ramprate = .25; final int current = 40; + final double ramprateFW = 5; + final int currentFW = 40; RobotContainer.l1.setSmartCurrentLimit(current); RobotContainer.l1.setClosedLoopRampRate(ramprate); RobotContainer.l1.setOpenLoopRampRate(ramprate); @@ -286,40 +263,55 @@ public void configRobot(){ RobotContainer.r2.setSmartCurrentLimit(current); RobotContainer.r2.setClosedLoopRampRate(ramprate); RobotContainer.r2.setOpenLoopRampRate(ramprate); - RobotContainer.flywheelMotor.setSmartCurrentLimit(current); - RobotContainer.flywheelMotor.setClosedLoopRampRate(ramprate+.75); - RobotContainer.flywheelMotor.setOpenLoopRampRate(ramprate+.75); - RobotContainer.flywheelMotor2.setSmartCurrentLimit(current); - RobotContainer.flywheelMotor2.setClosedLoopRampRate(ramprate+.75); - RobotContainer.flywheelMotor2.setOpenLoopRampRate(ramprate+.75); + RobotContainer.flywheelMotor.setSmartCurrentLimit(currentFW); + RobotContainer.flywheelMotor.setClosedLoopRampRate(ramprateFW); + RobotContainer.flywheelMotor.setOpenLoopRampRate(ramprateFW); + RobotContainer.flywheelMotor2.setSmartCurrentLimit(currentFW); + RobotContainer.flywheelMotor2.setClosedLoopRampRate(ramprateFW); + RobotContainer.flywheelMotor2.setOpenLoopRampRate(ramprateFW); RobotContainer.intakePistons.set(Value.kForward); + // RobotContainer.climbSub.pullWinch(); + RobotContainer.climbSub.freespinWinch(); + RobotContainer.climbSub.unlockClimb(); - // RobotContainer.turretMotor.overrideLimitSwitchesEnable(false); - // RobotContainer.turretMotor.overrideSoftLimitsEnable(false); - + RobotContainer.turretMotor.overrideLimitSwitchesEnable(false); + RobotContainer.turretMotor.overrideSoftLimitsEnable(false); } public void resetTheSpaghet(){ RobotContainer.turretMotor.getSensorCollection().setQuadraturePosition(0, 10); RobotContainer.hoodMotor.getSensorCollection().setQuadraturePosition(0, 10); // RobotContainer.hoodEncoder.resetAccumulator(); - RobotContainer.lift.getSensorCollection().setQuadraturePosition(0, 10); + // RobotContainer.lift.getSensorCollection().setQuadraturePosition(0, 10); RobotContainer.winch2.getSensorCollection().setQuadraturePosition(0, 10); - RobotContainer.l1.getEncoder().setPosition(0); - RobotContainer.l2.getEncoder().setPosition(0); - RobotContainer.r1.getEncoder().setPosition(0); - RobotContainer.r2.getEncoder().setPosition(0); + RobotContainer.driveSub.resetEncoders(); RobotContainer.flywheelMotor.getEncoder().setPosition(0); RobotContainer.flywheelMotor2.getEncoder().setPosition(0); RobotContainer.navX.reset(); + RobotContainer.driveSub.resetOdometry(); } public void shooter(){ - if(RobotContainer.manipulator.getPOV() == 90 || RobotContainer.manipulator.getPOV() == 180 || RobotContainer.manipulator.getPOV() == 0){ - // pt.schedule(); - ph.schedule(); + if(RobotContainer.manipulator.getPOV() == 0 || RobotContainer.manipulator.getPOV() == 90 || RobotContainer.manipulator.getPOV() == 180){ //RobotContainer.manipulator.getPOV() == 90 || RobotContainer.manipulator.getPOV() == 180 || + ph.schedule(true); + pt.schedule(true); + if(tc != null){ + tc.cancel(); + } + if(hc != null){ + hc.cancel(); + } + }else{ + if(ph != null){ + ph.cancel(); + hc.schedule(true); + } + if(pt != null){ + pt.cancel(); + tc.schedule(true); + } } } - + } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 458dd15..d3da1b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,12 +15,14 @@ import frc.robot.commands.FlywheelCommand; import frc.robot.commands.HoodCommand; import frc.robot.commands.IntakeCommand; -import frc.robot.commands.PIDTurret; +import frc.robot.commands.PositionControlCommand; +import frc.robot.commands.RotationControlCommand; +import frc.robot.commands.TurretCommand; import frc.robot.commands.climb.ClimbCommand; import frc.robot.commands.climb.ClimbSequence1; import frc.robot.commands.climb.ClimbSequence2; +import frc.robot.commands.climb.ClimbSequence3; import frc.robot.commands.climb.LockClimb; -import frc.robot.commands.climb.UnlockClimb; import frc.robot.subsystems.CPMSub; import frc.robot.subsystems.ClimbSub; import frc.robot.subsystems.DriveSub; @@ -30,7 +32,6 @@ import frc.robot.subsystems.LidarSub; import frc.robot.subsystems.TurretSub; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; public class RobotContainer { @@ -52,7 +53,7 @@ public class RobotContainer { public static WPI_TalonSRX intake3 = new WPI_TalonSRX(6); public static WPI_TalonSRX winch1 = new WPI_TalonSRX(7); - public static WPI_TalonSRX lift = new WPI_TalonSRX(8); + // public static WPI_TalonSRX lift = new WPI_TalonSRX(8); public static WPI_TalonSRX winch2 = new WPI_TalonSRX(9); //lift to -14000 for 12in\]] @@ -64,7 +65,7 @@ public class RobotContainer { // winchShift:kReverse=pull,kForward=freespin // intakePistons:kReverse=in,kForward=out // dSl:kReverse=low,kForward=high - public static Compressor compressor = new Compressor(0); + public static Compressor compressor = new Compressor(1); public static DoubleSolenoid dsL = new DoubleSolenoid(0, 0, 1); public static DoubleSolenoid intakePistons = new DoubleSolenoid(0, 2, 3); public static DoubleSolenoid winchShift = new DoubleSolenoid(0, 4, 5); @@ -133,13 +134,14 @@ public RobotContainer() { hoodSub.setDefaultCommand(new HoodCommand()); flywheelSub.setDefaultCommand(new FlywheelCommand()); climbSub.setDefaultCommand(new ClimbCommand()); + turretSub.setDefaultCommand(new TurretCommand()); + configureButtonBindings(); //lift.setDefaultCommand(new RunCommand( () -> lift.lift(manipulator.getRawAxis(5)) , lift)); // flywheelSub.setDefaultCommand(new RunCommand( () -> flywheelSub.spinFlywheel(driver.getRawAxis(1)), flywheelSub)); // hoodSub.setDefaultCommand(new RunCommand( () -> hoodSub.angleHood(driver.getRawAxis(5)), hoodSub)); - // turretSub.setDefaultCommand(new RunCommand( () -> turretSub.rotate(driver.getRawAxis(0)), flywheelSub)); } @@ -147,9 +149,13 @@ private void configureButtonBindings() { // ma.whileHeld(new PIDTurret()); + my.whileHeld(new PositionControlCommand()); + mx.whileHeld(new RotationControlCommand()); + da.whileHeld(new ClimbSequence1()); db.whileHeld(new ClimbSequence2()); db.whenReleased(new LockClimb()); + dstart.whileHeld(new ClimbSequence3()); } diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java index 29bc981..b2cc11e 100644 --- a/src/main/java/frc/robot/commands/AimCommand.java +++ b/src/main/java/frc/robot/commands/AimCommand.java @@ -14,11 +14,11 @@ public AimCommand(double hoodPos) { new ParallelCommandGroup( new PIDTurret(), new PIDHood(hoodPos), - new PIDFlywheel(16000) + new PIDFlywheel(16500) ), new ParallelCommandGroup( - new RunCommand( () -> RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed), RobotContainer.intakeSub), - new RunCommand( () -> RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed), RobotContainer.intakeSub), + new RunCommand( () -> RobotContainer.intakeSub.runTurretFeeder(IntakeSub.conveyorMotorSpeed), RobotContainer.intakeSub), + new RunCommand( () -> RobotContainer.intakeSub.runConveyorAndCPM(IntakeSub.starWheelAndCPMSpeed), RobotContainer.intakeSub), new PIDFlywheel() ) diff --git a/src/main/java/frc/robot/commands/AutoIntakeIntake.java b/src/main/java/frc/robot/commands/AutoIntakeIntake.java new file mode 100644 index 0000000..e272ecf --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoIntakeIntake.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.IntakeSub; + +public class AutoIntakeIntake extends CommandBase { + + public AutoIntakeIntake() { + addRequirements(RobotContainer.intakeSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println("Intake intake Command init"); + SmartDashboard.putBoolean("Exist", true); + SmartDashboard.putBoolean("Not Exist", false); + SmartDashboard.putBoolean("Exist Execute", false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + SmartDashboard.putBoolean("Exist Execute", true); + System.out.println("Intake intake Command exe"); + RobotContainer.intakeSub.intakeOut(); + RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.intakeSub.runIntake(0); + SmartDashboard.putBoolean("Not Exist", true); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AutoShootIntake.java b/src/main/java/frc/robot/commands/AutoShootIntake.java new file mode 100644 index 0000000..ea0c440 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootIntake.java @@ -0,0 +1,93 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.RobotContainer; +import frc.robot.subsystems.FlywheelSub; +import frc.robot.subsystems.IntakeSub; + +public class AutoShootIntake extends CommandBase { + + boolean runConveyor = false; + boolean finalBall = false; + int numBall = 0; + boolean shoot; + boolean intaking; + boolean startTimer = false; + double tim = 0; + Timer t = new Timer(); + public static boolean finished = false; + + public AutoShootIntake(boolean intaking) { + addRequirements(RobotContainer.intakeSub, RobotContainer.flywheelSub); + this.intaking = intaking; + } + + @Override + public void initialize() { + t.reset(); + shoot = false; + countBalls = 0; + finished = false; + RobotContainer.intakeSub.intakeIn(); + } + + @Override + public void execute() { + System.out.println("Intaking: " + intaking); + if(!intaking){ + RobotContainer.flywheelSub.spinFlywheels(1); + if(RobotContainer.flywheelSub.getFlywheelSpeed() > 16000) {//16000 + shoot = true; + if(!startTimer){ + t.start(); + startTimer = true; + tim = DriverStation.getInstance().getMatchTime(); + } + } + + if(shoot){ + RobotContainer.intakeSub.runConveyorAndCPM(IntakeSub.starWheelAndCPMSpeed); + RobotContainer.intakeSub.runTurretFeeder(IntakeSub.conveyorMotorSpeed); + } + }else{ + RobotContainer.intakeSub.intakeOut(); + RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed); + } + if(t.get() != 0){ + System.out.println("Timer is: " + t.get()); + } + + } + + @Override + public void end(boolean interrupted) { + RobotContainer.intakeSub.runConveyorAndCPM(0); + RobotContainer.intakeSub.runTurretFeeder(0); + RobotContainer.intakeSub.intakeOut(); + finished = true; + } + + @Override + public boolean isFinished() { + return countBalls()>=3 || t.get()>5 || DriverStation.getInstance().getMatchTime()-tim > 5; + } + + int countBalls = 0; + boolean prevTrue = false; + public int countBalls(){ + if(!prevTrue && RobotContainer.touchThree.get()){ + countBalls++; + } + + if(!RobotContainer.touchThree.get()) prevTrue = false; + else prevTrue = true; + + System.out.println("counting: " + countBalls); + System.out.println("prevTrue: " + prevTrue); + return countBalls; + } +} diff --git a/src/main/java/frc/robot/commands/CPMCommand.java b/src/main/java/frc/robot/commands/CPMCommand.java index 2f94b11..bdea726 100644 --- a/src/main/java/frc/robot/commands/CPMCommand.java +++ b/src/main/java/frc/robot/commands/CPMCommand.java @@ -1,10 +1,3 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index a5389a6..d59a431 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -1,10 +1,14 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; import frc.robot.RobotContainer; public class DriveCommand extends CommandBase { + boolean autoShift = true; + public DriveCommand() { addRequirements(RobotContainer.driveSub); } @@ -16,14 +20,30 @@ public void initialize() { @Override public void execute() { - // RobotContainer.driveSub.drive(RobotContainer.driver.getRawAxis(1), 0.6 * RobotContainer.driver.getRawAxis(4)); RobotContainer.driveSub.teleDrive(); - // if (RobotContainer.driveSub.getCurrentGear() == 1 && Math.abs(RobotContainer.driveSub.getRotationSpeed(RobotContainer.driveSub.getCurrentGear())) > DriveSub.limitRotSpdGear1) { - // RobotContainer.driveSub.shift(); - // } else if (RobotContainer.driveSub.getCurrentGear() == 2 && Math.abs(RobotContainer.driveSub.getRotationSpeed(RobotContainer.driveSub.getCurrentGear())) < DriveSub.limitRotSpdGear1) { - // RobotContainer.driveSub.shift(); - // } + if(RobotContainer.driver.getPOV() == 180){ + autoShift = false; + } + if(RobotContainer.driver.getPOV() == 0){ + autoShift = true; + } + if(!Robot.auto){ + if(autoShift){ + if (RobotContainer.dsL.get() == Value.kReverse && Math.abs(RobotContainer.driveSub.getWheelVelocity())>2.1){ //1.8 + RobotContainer.dsL.set(Value.kForward); + } else if (RobotContainer.dsL.get() == Value.kForward && Math.abs(RobotContainer.driveSub.getWheelVelocity()) < 0.8) { //1.2 + RobotContainer.dsL.set(Value.kReverse); + } + }else{ + if(RobotContainer.driver.getPOV() == 90){ + RobotContainer.dsL.set(Value.kForward); + }else if(RobotContainer.driver.getPOV() == 270){ + RobotContainer.dsL.set(Value.kReverse); + } + } + } + } @Override diff --git a/src/main/java/frc/robot/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index eefa84a..0498cd5 100644 --- a/src/main/java/frc/robot/commands/FlywheelCommand.java +++ b/src/main/java/frc/robot/commands/FlywheelCommand.java @@ -1,10 +1,3 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc/robot/commands/HoodCommand.java b/src/main/java/frc/robot/commands/HoodCommand.java index 33e19c7..08d6722 100644 --- a/src/main/java/frc/robot/commands/HoodCommand.java +++ b/src/main/java/frc/robot/commands/HoodCommand.java @@ -1,40 +1,26 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RobotContainer; public class HoodCommand extends CommandBase { - /** - * Creates a new HoodCommand. - */ public HoodCommand() { addRequirements(RobotContainer.hoodSub); } - // Called when the command is initially scheduled. @Override public void initialize() { } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { RobotContainer.hoodMotor.set(RobotContainer.manipulator.getRawAxis(1)); } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { } - // Returns true when the command should end. @Override public boolean isFinished() { return false; diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 39193f6..6421d5f 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -4,12 +4,18 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.RobotContainer; +import frc.robot.subsystems.FlywheelSub; import frc.robot.subsystems.IntakeSub; public class IntakeCommand extends CommandBase { + boolean runConveyor = false; + boolean finalBall = false; + int numBall=0; + boolean shoot; public IntakeCommand() { addRequirements(RobotContainer.intakeSub); + shoot=false; } @Override @@ -18,80 +24,115 @@ public void initialize() { @Override public void execute() { - - if(RobotContainer.touchOne.get() && RobotContainer.touchThree.get()){ - IntakeSub.starWheelOff = true; - } - if(RobotContainer.manipulator.getRawAxis(3) > 0.1){ - IntakeSub.starWheelOff = false; - } - + if(RobotContainer.flywheelSub.getFlywheelSpeed() > 16000) shoot = true;//16000 + else shoot = false; double axis2 = RobotContainer.manipulator.getRawAxis(2); - // if(RobotContainer.manipulator.getPOV() == 180){ - // RobotContainer.intakeSub.intakeOut(); - // } - - // if(RobotContainer.manipulator.getPOV() == 0){ - // RobotContainer.intakeSub.intakeIn(); - // } + SmartDashboard.putBoolean("Conveyor running", runConveyor); if(RobotContainer.ma.get()){ - System.out.println("1"); RobotContainer.intakeSub.intakeOut(); - if(IntakeSub.starWheelOff){ - System.out.println("2"); - RobotContainer.intakeSub.runStarWheelAndCPM(0); + if(RobotContainer.mb.get()){ + RobotContainer.intakeSub.runConveyorAndCPM(IntakeSub.intakeSpeed*-.5); + }else if(!IntakeSub.cyclingBall){ + RobotContainer.intakeSub.runConveyorAndCPM(IntakeSub.intakeSpeed); }else{ - System.out.println("3"); - RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.intakeSpeed); + RobotContainer.intakeSub.runConveyorAndCPM(0); } if(RobotContainer.mb.get()){ - System.out.println("4"); RobotContainer.intakeSub.runIntake(-IntakeSub.intakeSpeed); }else{ - System.out.println("5"); RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed); } } else if (!RobotContainer.ma.get()){ - System.out.println("6"); - RobotContainer.intakeSub.runStarWheelAndCPM(0); + RobotContainer.intakeSub.runConveyorAndCPM(0); RobotContainer.intakeSub.runIntake(0); - // RobotContainer.intakeSub.intakeIn(); } - // if(axis2 > 0.5 && (RobotContainer.flywheelSub.getFlywheelSpeed() > 15000)){ - // RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed); - // }else{ - // RobotContainer.intakeSub.runConveyorBelt(0); - // } + if(axis2 > 0.5 && shoot){ //&& RobotContainer.flywheelSub.getFlywheelSpeed() > 15000 + RobotContainer.intakeSub.runTurretFeeder(IntakeSub.conveyorMotorSpeed); + }else if(RobotContainer.manipulator.getPOV() == 270){ + RobotContainer.intakeSub.runTurretFeeder(0); + IntakeSub.targetTS = 3; + IntakeSub.cyclingBall = false; + } + + if(RobotContainer.touchOne.get() && RobotContainer.touchTwo.get() && RobotContainer.touchThree.get()){ + finalBall = true; + } + + if(RobotContainer.manipulator.getRawAxis(3) > 0.5){ + finalBall = false; + } - if (RobotContainer.touchOne.get() && !RobotContainer.touchThree.get() && axis2 < 0.5) { + if (RobotContainer.touchOne.get() && !RobotContainer.touchTwo.get() && axis2 < 0.5) {//axis2 is manual conveyor belt, not intake IntakeSub.cyclingBall = true; + }else if(axis2 > 0.5){ + IntakeSub.cyclingBall = false; } - if (IntakeSub.cyclingBall) { - if (IntakeSub.targetTS == 2) { - if (!RobotContainer.touchTwo.get()) { - RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed); - if(!IntakeSub.starWheelOff) RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed); - } else { - RobotContainer.intakeSub.runConveyorBelt(0); - RobotContainer.intakeSub.runStarWheelAndCPM(0);; - IntakeSub.targetTS = 3; - IntakeSub.cyclingBall = false; - } - } else if (IntakeSub.targetTS == 3) { - if (!RobotContainer.touchThree.get()) { - RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed); - } else { - RobotContainer.intakeSub.runConveyorBelt(0); - IntakeSub. targetTS = 2; - IntakeSub.cyclingBall = false; + if(RobotContainer.touchOne.get() && RobotContainer.touchTwo.get() && RobotContainer.touchThree.get()) numBall = 3; + else if(!RobotContainer.touchOne.get() && RobotContainer.touchTwo.get() && RobotContainer.touchThree.get() || + RobotContainer.touchOne.get() && !RobotContainer.touchTwo.get() && RobotContainer.touchThree.get() || + RobotContainer.touchOne.get() && RobotContainer.touchTwo.get() && !RobotContainer.touchThree.get() + ) numBall = 2; + else if(!RobotContainer.touchOne.get() && !RobotContainer.touchTwo.get() && RobotContainer.touchThree.get() || + !RobotContainer.touchOne.get() && RobotContainer.touchTwo.get() && !RobotContainer.touchThree.get() || + RobotContainer.touchOne.get() && !RobotContainer.touchTwo.get() && !RobotContainer.touchThree.get() + ) numBall = 1; + else numBall = 0; + + if(numBall == 0 || numBall == 1){ + IntakeSub.targetTS = 3; + } + SmartDashboard.putNumber("TargetTS", IntakeSub.targetTS); + + if(RobotContainer.touchOne.get()){ + System.out.println("Touched 1"); + } + + if(RobotContainer.touchTwo.get()){ + System.out.println("Touched 2"); + } + + if(RobotContainer.touchThree.get()){ + System.out.println("Touched 3"); + } + + if(numBall != 3){ + if(finalBall){ + RobotContainer.intakeSub.runConveyorAndCPM(0); + RobotContainer.intakeSub.runTurretFeeder(0); + }else{ + if (IntakeSub.cyclingBall && !(RobotContainer.manipulator.getRawAxis(3) > 0.5)) { + if(numBall == 1 && RobotContainer.touchOne.get()){ + RobotContainer.intakeSub.runIntake(0); + }else if(numBall == 2 && RobotContainer.touchOne.get()){ + RobotContainer.intakeSub.runIntake(0); + } + if (IntakeSub.targetTS == 2 && RobotContainer.touchThree.get()) { + if (!RobotContainer.touchTwo.get()) { + RobotContainer.intakeSub.runConveyorAndCPM(IntakeSub.starWheelAndCPMSpeed); + } else { + RobotContainer.intakeSub.runConveyorAndCPM(0); + IntakeSub.cyclingBall = false; + } + } else if (IntakeSub.targetTS == 3) { + if (!RobotContainer.touchThree.get()) { + RobotContainer.intakeSub.runConveyorAndCPM(IntakeSub.starWheelAndCPMSpeed); + RobotContainer.intakeSub.runTurretFeeder(IntakeSub.conveyorMotorSpeed*.4); + } else { + RobotContainer.intakeSub.runConveyorAndCPM(0); + RobotContainer.intakeSub.runTurretFeeder(0); + IntakeSub. targetTS = 2; + IntakeSub.cyclingBall = false; + } + } } } } + } @Override diff --git a/src/main/java/frc/robot/commands/PIDFlywheel.java b/src/main/java/frc/robot/commands/PIDFlywheel.java index 4e0c078..1935493 100644 --- a/src/main/java/frc/robot/commands/PIDFlywheel.java +++ b/src/main/java/frc/robot/commands/PIDFlywheel.java @@ -8,8 +8,8 @@ public class PIDFlywheel extends CommandBase { private static PIDController pidController; - private static final double kP = 0; - private static final double kI = 0; + private static final double kP = 0.00011; + private static final double kI = 0.0001; private static final double kD = 0; private static final double tolerance = 1; public double setpoint; @@ -40,6 +40,7 @@ public void initialize() { @Override public void execute() { double output = pidController.calculate(RobotContainer.flywheelSub.getFlywheelSpeed(), setpoint); + System.out.println(setpoint); RobotContainer.flywheelSub.spinFlywheels(output); } @@ -49,11 +50,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - if(pidController.atSetpoint()) { - flywheelAtSetpoint = true; - } else { - flywheelAtSetpoint = false; - } - return pidController.atSetpoint(); + // return pidController.atSetpoint(); + return false; } } diff --git a/src/main/java/frc/robot/commands/PIDHood.java b/src/main/java/frc/robot/commands/PIDHood.java index 4a2725f..123ea5a 100644 --- a/src/main/java/frc/robot/commands/PIDHood.java +++ b/src/main/java/frc/robot/commands/PIDHood.java @@ -9,8 +9,8 @@ public class PIDHood extends CommandBase { PIDController pidController; - private final double kP = 0.0015; - private final double kI = 0; + private final double kP = 0.0014; + private final double kI = 0.0013; private final double kD = 0; private final double tolerance = 7; private double setpoint; @@ -45,6 +45,7 @@ public void execute() { hoodPosition(); System.out.println("going with the PID hood"); SmartDashboard.putNumber("Hood error!", pidController.getPositionError()); + SmartDashboard.putNumber("Hood setpoint", pidController.getSetpoint()); double output = -pidController.calculate(RobotContainer.hoodSub.getHoodPos(), setpoint); SmartDashboard.putNumber("Hood PID output", output); diff --git a/src/main/java/frc/robot/commands/PIDTurret.java b/src/main/java/frc/robot/commands/PIDTurret.java index 49eb93f..b378202 100644 --- a/src/main/java/frc/robot/commands/PIDTurret.java +++ b/src/main/java/frc/robot/commands/PIDTurret.java @@ -13,7 +13,7 @@ public class PIDTurret extends CommandBase { static PIDController pidController; private static final double kP = 0.025; - private static final double kI = 0; + private static final double kI = 0.0001; private static final double kD = 0; private static final double tolerance = 2; @@ -22,18 +22,26 @@ public class PIDTurret extends CommandBase { static NetworkTable table; static NetworkTableEntry tx; static NetworkTableEntry tv; - private static double setpoint = 0; + private static double setpoint = -2; public PIDTurret() { table = NetworkTableInstance.getDefault().getTable("limelight"); addRequirements(RobotContainer.turretSub); } + public PIDTurret(double setpoint) { + table = NetworkTableInstance.getDefault().getTable("limelight"); + addRequirements(RobotContainer.turretSub); + this.setpoint = setpoint; + } + public void turretPosition(){ if(RobotContainer.manipulator.getPOV() == 90){ setpoint = Constants.turretFromTrench; }else if(RobotContainer.manipulator.getPOV() == 180){ - setpoint = Constants.turretClose; + setpoint = Constants.turretLine; + }else if(RobotContainer.manipulator.getPOV() == 0){ + setpoint = Constants.turretTriangle; } } @@ -62,6 +70,7 @@ public void execute() { SmartDashboard.putNumber("This is PID turret output", output); SmartDashboard.putNumber("This is PID turret error!!", pidController.getPositionError()); + SmartDashboard.putNumber("This is PID turret setpoint!", pidController.getSetpoint()); } @Override diff --git a/src/main/java/frc/robot/commands/PositionControlCommand.java b/src/main/java/frc/robot/commands/PositionControlCommand.java new file mode 100644 index 0000000..91648c1 --- /dev/null +++ b/src/main/java/frc/robot/commands/PositionControlCommand.java @@ -0,0 +1,70 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.CPMSub; + +public class PositionControlCommand extends CommandBase { + + String target; + String currentColor; + String[] colors1 = {"B","G","R","Y"}; + String[] colors2 = {"R","Y","B","G"}; + int numOfColorsAway; + public PositionControlCommand() { + } + + @Override + public void initialize() { + target = CPMSub.getTargetColor(); + currentColor = CPMSub.getColorL(); + if(target == "G" || target == "R") { + + numOfColorsAway = CPMSub.findMatchingColorIndex(colors1, target) - CPMSub.findMatchingColorIndex(colors1, currentColor); + + } else if(target == "Y" || target == "B"){ + + numOfColorsAway = CPMSub.findMatchingColorIndex(colors2, target) - CPMSub.findMatchingColorIndex(colors2, currentColor); + + } else { + numOfColorsAway = 100000000; + } + + } + + @Override + public void execute() { + + RobotContainer.cpmSub.printGetting(); + + SmartDashboard.putString("Get Color L", CPMSub.getColorL().toString()); + SmartDashboard.putString("Get Color R", CPMSub.getColorR().toString()); + + // SmartDashboard.putString("Get Target Color", CPMSub.getTargetColor()); + + RobotContainer.intake2.set(-0.3); + //spin cpm motor + // if (numOfColorsAway != 100000000){ + // if (numOfColorsAway <= 0) { + // RobotContainer.cpmSub.runCPMAndIntake(CPMSub.CPMSpeed); + // } else if (numOfColorsAway >= 0) { + // RobotContainer.cpmSub.runCPMAndIntake(-CPMSub.CPMSpeed); + // } + // } + } + + @Override + public void end(boolean interrupted) { + + } + + @Override + public boolean isFinished() { + if (CPMSub.getColorL() == target && CPMSub.getColorR() == target) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/RotationControlCommand.java b/src/main/java/frc/robot/commands/RotationControlCommand.java new file mode 100644 index 0000000..d624fdb --- /dev/null +++ b/src/main/java/frc/robot/commands/RotationControlCommand.java @@ -0,0 +1,65 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.CPMSub; + +public class RotationControlCommand extends CommandBase { + + String startingColor; + int numberOfRevs; + double timeout = 0.5; + Timer t = new Timer(); + + public RotationControlCommand() { + addRequirements(RobotContainer.cpmSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + startingColor = CPMSub.getColorL(); + numberOfRevs = 0; + t.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + RobotContainer.cpmSub.printGetting(); + + SmartDashboard.putString("Get Color L", CPMSub.getColorL().toString()); + SmartDashboard.putString("Get Color R", CPMSub.getColorR().toString()); + + // SmartDashboard.putString("Get Target Color", CPMSub.getTargetColor()); + + SmartDashboard.putNumber("Nymber of revs", numberOfRevs); + + if(numberOfRevs < 3){ + RobotContainer.intake2.set(0.3); + //spin cpm motor + } + if (t.hasElapsed(timeout) && CPMSub.getColorL() == startingColor) { + numberOfRevs+= 0.5; + t.reset(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if(numberOfRevs == 3) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TurretCommand.java b/src/main/java/frc/robot/commands/TurretCommand.java new file mode 100644 index 0000000..c6c5181 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurretCommand.java @@ -0,0 +1,28 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class TurretCommand extends CommandBase { + public TurretCommand() { + addRequirements(RobotContainer.turretSub); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + RobotContainer.turretMotor.set(-RobotContainer.manipulator.getRawAxis(4) * 0.2); + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TurretFeeder.java b/src/main/java/frc/robot/commands/TurretFeeder.java index df6eea0..0d947b6 100644 --- a/src/main/java/frc/robot/commands/TurretFeeder.java +++ b/src/main/java/frc/robot/commands/TurretFeeder.java @@ -17,7 +17,7 @@ public void initialize() { @Override public void execute() { if (PIDFlywheel.flywheelAtSetpoint) { - RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed); + RobotContainer.intakeSub.runTurretFeeder(IntakeSub.conveyorMotorSpeed); } } diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/auto/AutoDistanceDriveCommand.java similarity index 58% rename from src/main/java/frc/robot/commands/drive/DriveCommand.java rename to src/main/java/frc/robot/commands/auto/AutoDistanceDriveCommand.java index 37192c3..4a054aa 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommand.java +++ b/src/main/java/frc/robot/commands/auto/AutoDistanceDriveCommand.java @@ -5,22 +5,19 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands.drive; +package frc.robot.commands.auto; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Robot; import frc.robot.RobotContainer; -import frc.robot.subsystems.DriveSub; -public class DriveCommand extends CommandBase { +public class AutoDistanceDriveCommand extends CommandBase { + private double distance; - public DriveCommand() { - // Use addRequirements() here to declare subsystem dependencies. + public AutoDistanceDriveCommand() { addRequirements(RobotContainer.driveSub); } - // Called when the command is initially scheduled. @Override public void initialize() { } @@ -28,14 +25,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - RobotContainer.driveSub.teleDrive(); - - // if (RobotContainer.sol1.get() == Value.kForward && Math.abs(Robot.driveSub.getWheelVelocity()) > 2.3) { - // RobotContainer.sol1.set(Value.kReverse); - // } else if (RobotContainer.sol1.get() == Value.kReverse && Math.abs(Robot.driveSub.getWheelVelocity()) < 3) { - // RobotContainer.sol1.set(Value.kForward); - // } + SmartDashboard.putNumber("this is the encoder of r1", RobotContainer.r1.getEncoder().getPosition()); + RobotContainer.driveSub.drive(0.5, 0); } // Called once the command ends or is interrupted. @@ -46,6 +37,9 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { + if(RobotContainer.r1.getEncoder().getPosition() > 35){ + return true; + } return false; } } diff --git a/src/main/java/frc/robot/commands/auto/AutoIntaking.java b/src/main/java/frc/robot/commands/auto/AutoIntaking.java new file mode 100644 index 0000000..e904579 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoIntaking.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.AutoIntakeIntake; +import frc.robot.commands.AutoShootIntake; +import frc.robot.commands.auto.paths.Move; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class AutoIntaking extends SequentialCommandGroup { + /** + * Creates a new AutoIntaking. + */ + public AutoIntaking() { + + addCommands( + // new AutoDistanceDriveCommand(), + new BasicAuto(), + // new Move(1, false).withTimeout(Move.commandTime), + new ParallelCommandGroup( + new AutoIntakeIntake(), + new Move(1, false).withTimeout(Move.commandTime) + ) + ); + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + } +} diff --git a/src/main/java/frc/robot/commands/auto/BasicAuto.java b/src/main/java/frc/robot/commands/auto/BasicAuto.java index 1bfcf85..d466d08 100644 --- a/src/main/java/frc/robot/commands/auto/BasicAuto.java +++ b/src/main/java/frc/robot/commands/auto/BasicAuto.java @@ -1,27 +1,27 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; import frc.robot.RobotContainer; +import frc.robot.commands.AutoIntakeIntake; +import frc.robot.commands.AutoShootIntake; import frc.robot.commands.PIDHood; import frc.robot.commands.PIDTurret; -import frc.robot.commands.auto.paths.Forward; +import frc.robot.commands.auto.paths.Move; +import frc.robot.subsystems.FlywheelSub; public class BasicAuto extends SequentialCommandGroup { - /** - * Creates a new BasicAuto. - */ public BasicAuto() { - addCommands( - new ParallelCommandGroup( - new Forward(2), - new PIDTurret(), - new PIDHood(Constants.hoodTriangle)), - new RunCommand( () -> RobotContainer.flywheelSub.spinFlywheels(1), RobotContainer.flywheelSub) - ); - + new ParallelDeadlineGroup( + new AutoShootIntake(false).withInterrupt(() -> AutoShootIntake.finished), + new PIDTurret(Constants.turretLine), + new PIDHood(Constants.hoodAutoLine) + ) + ); } } diff --git a/src/main/java/frc/robot/commands/auto/ThisIsDumb.java b/src/main/java/frc/robot/commands/auto/ThisIsDumb.java new file mode 100644 index 0000000..552460c --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/ThisIsDumb.java @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class ThisIsDumb extends SequentialCommandGroup { + + public ThisIsDumb() { + addCommands( + // new BasicAuto(), + new AutoIntaking() + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/paths/Chain.java b/src/main/java/frc/robot/commands/auto/paths/Chain.java index 9f0594c..8706fc4 100644 --- a/src/main/java/frc/robot/commands/auto/paths/Chain.java +++ b/src/main/java/frc/robot/commands/auto/paths/Chain.java @@ -14,7 +14,7 @@ public class Chain extends SequentialCommandGroup { */ public Chain() throws IOException { addCommands( - new Forward(1).withTimeout(Forward.commandTime), + new Move(1,false).withTimeout(Move.commandTime), new TurnReverse() ); } diff --git a/src/main/java/frc/robot/commands/auto/paths/Forward.java b/src/main/java/frc/robot/commands/auto/paths/Move.java similarity index 86% rename from src/main/java/frc/robot/commands/auto/paths/Forward.java rename to src/main/java/frc/robot/commands/auto/paths/Move.java index fd73897..4a89a94 100644 --- a/src/main/java/frc/robot/commands/auto/paths/Forward.java +++ b/src/main/java/frc/robot/commands/auto/paths/Move.java @@ -1,6 +1,8 @@ package frc.robot.commands.auto.paths; import java.util.List; + +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; @@ -15,8 +17,9 @@ import frc.robot.RobotContainer; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.IntakeSub; -public class Forward extends CommandBase { +public class Move extends CommandBase { public static double commandTime = 0; double dist = 1; DifferentialDriveVoltageConstraint autoVoltageConstraint = new DifferentialDriveVoltageConstraint( @@ -51,30 +54,33 @@ public class Forward extends CommandBase { RobotContainer.driveSub ); - public Forward(double dist) { + public Move(double dist, boolean reverse) { this.dist = dist; commandTime = exampleTrajectory.getTotalTimeSeconds(); addRequirements(RobotContainer.driveSub); + DriveConstants.reverse = reverse; } @Override public void initialize() { - DriveConstants.reverse = false; + RobotContainer.intakePistons.set(Value.kReverse); + RobotContainer.intake1.set(IntakeSub.intakeSpeed); gordonRamsete.schedule(); } @Override public void execute() { + RobotContainer.intake1.set(IntakeSub.intakeSpeed); // System.out.println("LEFT: " + gordonRamsete.leftSpeedSetpoint); // System.out.println("RIGHT: " + gordonRamsete.rightSpeedSetpoint); - // System.out.println("SPEEDS: " + Robot.driveSub.getWheelSpeeds().toString()); + System.out.println("SPEEDS: " + RobotContainer.driveSub.getWheelSpeeds().toString()); } @Override public void end(boolean interrupted) { RobotContainer.left.set(0); RobotContainer.right.set(0); - } + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/climb/AutoLift.java b/src/main/java/frc/robot/commands/climb/AutoLift.java index b85f711..218f2b1 100644 --- a/src/main/java/frc/robot/commands/climb/AutoLift.java +++ b/src/main/java/frc/robot/commands/climb/AutoLift.java @@ -1,22 +1,15 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot.commands.climb; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RobotContainer; public class AutoLift extends CommandBase { - /** - * Creates a new AutoLift. - */ boolean up2; + boolean first = false; double count2; + Timer t = new Timer(); public AutoLift(double count, boolean up) { addRequirements(RobotContainer.climbSub); count2=count;//-14000 to work @@ -27,6 +20,7 @@ public AutoLift(double count, boolean up) { // Called when the command is initially scheduled. @Override public void initialize() { + t.reset(); } // Called every time the scheduler runs while the command is scheduled. @@ -34,7 +28,12 @@ public void initialize() { public void execute() { SmartDashboard.putBoolean("up in command", up2); System.out.println("climb go"); - RobotContainer.climbSub.autoLift(count2, up2); + // if(RobotContainer.lift.getSensorCollection().getQuadratureVelocity() == 0 && t.get() > 3) { + // RobotContainer.climbSub.autoLift(0, up2); + // } + // else RobotContainer.climbSub.autoLift(count2, up2); + + // RobotContainer.climbSub.autoLift(count2, up2); } // Called once the command ends or is interrupted. @@ -45,11 +44,11 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14000 ){//&& count2 == 14000){ - return false; - }else if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -13000){ //&& count2 == 13000){ - return false; - } + // if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14000 ){//&& count2 == 14000){ + // return false; + // }else if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -13000){ //&& count2 == 13000){ + // return false; + // } return true; } } diff --git a/src/main/java/frc/robot/commands/climb/ClimbSequence3.java b/src/main/java/frc/robot/commands/climb/ClimbSequence3.java new file mode 100644 index 0000000..ab98a03 --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/ClimbSequence3.java @@ -0,0 +1,21 @@ +package frc.robot.commands.climb; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class ClimbSequence3 extends SequentialCommandGroup { + /** + * Creates a new ClimbSequence. + */ + public ClimbSequence3() { + addCommands( + new UnlockClimb(), + new FreespinWinch(), + new AutoLift(-10000, false) + ); + + + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + } +} diff --git a/src/main/java/frc/robot/subsystems/CPMSub.java b/src/main/java/frc/robot/subsystems/CPMSub.java index 91e83b5..d932a04 100644 --- a/src/main/java/frc/robot/subsystems/CPMSub.java +++ b/src/main/java/frc/robot/subsystems/CPMSub.java @@ -4,14 +4,17 @@ import com.revrobotics.ColorMatchResult; import com.revrobotics.ColorSensorV3; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; public class CPMSub extends SubsystemBase { private static final I2C.Port i2c1 = I2C.Port.kOnboard; - private static final I2C.Port i2c2 = I2C.Port.kOnboard; + private static final I2C.Port i2c2 = I2C.Port.kMXP; private static final ColorSensorV3 l_colorSensor = new ColorSensorV3(i2c1); private static final ColorSensorV3 r_colorSensor = new ColorSensorV3(i2c2); @@ -19,75 +22,149 @@ public class CPMSub extends SubsystemBase { private static final ColorMatch l_colorMatcher = new ColorMatch(); private static final ColorMatch r_colorMatcher = new ColorMatch(); - public static final Color kBlueTarget = ColorMatch.makeColor(0.143, 0.427, 0.429); - public static final Color kGreenTarget = ColorMatch.makeColor(0.197, 0.561, 0.240); - public static final Color kRedTarget = ColorMatch.makeColor(0.458, 0.375, 0.135); - public static final Color kYellowTarget = ColorMatch.makeColor(0.361, 0.524, 0.113); + Color detectedColorRight; + Color detectedColorLeft; + public static final Color kBlueTargetRight = ColorMatch.makeColor(0.193, 0.361, 0.400); //R: 206 G: 83 B: 113 //0.193, 0.361, 0.400 + // public static final Color kGreenTargetRight = ColorMatch.makeColor(0.197, 0.561, 0.240); + public static final Color kGreenTargetRight = ColorMatch.makeColor(0.227, 0.251, 0.511); //R: 17 G: 205 B: 155 //0.227, 0.251, 0.511 + public static final Color kRedTargetRight = ColorMatch.makeColor(0.227, 0.251, 0.511); //16,21,198) /0.227, 0.251, 0.511 + public static final Color kYellowTargetRight = ColorMatch.makeColor(0.283, 0.188, 0.101); //R: 27 G: 200 B: 101 - public CPMSub() { - l_colorMatcher.addColorMatch(kBlueTarget); - l_colorMatcher.addColorMatch(kGreenTarget); - l_colorMatcher.addColorMatch(kRedTarget); - l_colorMatcher.addColorMatch(kYellowTarget); - - r_colorMatcher.addColorMatch(kBlueTarget); - r_colorMatcher.addColorMatch(kGreenTarget); - r_colorMatcher.addColorMatch(kRedTarget); - r_colorMatcher.addColorMatch(kYellowTarget); + public static final Color kBlueTargetLeft = ColorMatch.makeColor(0.143, 0.427, 0.429); + public static final Color kGreenTargetLeft = ColorMatch.makeColor(0.197, 0.561, 0.240); + public static final Color kRedTargetLeft = ColorMatch.makeColor(0.458, 0.375, 0.135); + public static final Color kYellowTargetLeft = ColorMatch.makeColor(0.361, 0.524, 0.113); + public static final double starWheelSpeed = 0.6; + public static final double CPMSpeed = 0.5; + + public static final String[] colors1 = { "B", "G", "R", "Y" }; + public static final String[] colors2 = { "R", "Y", "B", "G" }; + + public CPMSub() { + l_colorMatcher.addColorMatch(kBlueTargetLeft); + l_colorMatcher.addColorMatch(kGreenTargetLeft); + l_colorMatcher.addColorMatch(kRedTargetLeft); + l_colorMatcher.addColorMatch(kYellowTargetLeft); + + r_colorMatcher.addColorMatch(kBlueTargetRight); + r_colorMatcher.addColorMatch(kGreenTargetRight); + r_colorMatcher.addColorMatch(kRedTargetRight); + r_colorMatcher.addColorMatch(kYellowTargetRight); + + } + + public void printGetting(){ + + detectedColorRight = r_colorSensor.getColor(); + detectedColorLeft = l_colorSensor.getColor(); + + SmartDashboard.putNumber("Left Red detect", detectedColorRight.red); + SmartDashboard.putNumber("Left Blue detect", detectedColorRight.blue); + SmartDashboard.putNumber("Left Green detect", detectedColorRight.green); + SmartDashboard.putNumber("Right Red detect", detectedColorLeft.red); + SmartDashboard.putNumber("Right Blue detect", detectedColorLeft.blue); + SmartDashboard.putNumber("Right Green detect", detectedColorLeft.green); + + SmartDashboard.putNumber("Left Red", l_colorSensor.getRed()); + SmartDashboard.putNumber("Left Blue", l_colorSensor.getBlue()); + SmartDashboard.putNumber("Left Green", l_colorSensor.getGreen()); + SmartDashboard.putNumber("Right Red", r_colorSensor.getRed()); + SmartDashboard.putNumber("Right Blue", r_colorSensor.getBlue()); + SmartDashboard.putNumber("Right Green", r_colorSensor.getGreen()); + } - public static String getColorL(){ + public static String getColorL() { Color color = l_colorSensor.getColor(); ColorMatchResult result = l_colorMatcher.matchClosestColor(color); String returnable; - if (result.color == kBlueTarget) { + // SmartDashboard.putString("resulted color!!", result.color.toString()); + + if (result.color == kBlueTargetLeft) { + // if(result.color == kGreenTargetLeft){ + // returnable = "Green"; + // }else{ returnable = "Blue"; - } else if (result.color == kRedTarget) { + // } + } else if (result.color == kRedTargetLeft) { + // if(result.color == kYellowTargetLeft){ + // returnable = "Yellow"; + // }else{ returnable = "Red"; - } else if (result.color == kGreenTarget) { - returnable = "Green"; - } else if (result.color == kYellowTarget) { - returnable = "Yellow"; - } else { - returnable = "Unknown"; - } + // } + } else if (result.color == kGreenTargetLeft) { + returnable = "Green"; + } else if (result.color == kYellowTargetLeft) { + returnable = "Yellow"; + } else { + returnable = "Unknown"; + } - return returnable; - + return returnable; + + } + + public void runCPMAndIntake(double speed) { + RobotContainer.intake2.set(speed); } - public static String getColorR(){ + public static String getColorR() { Color color = r_colorSensor.getColor(); ColorMatchResult result = r_colorMatcher.matchClosestColor(color); String returnable; - if (result.color == kBlueTarget) { + SmartDashboard.putString("Right result match", r_colorMatcher.matchClosestColor(color).toString()); + SmartDashboard.putString("resulted color!! Right", result.color.toString()); + + + if (result.color == kBlueTargetRight) { returnable = "Blue"; - } else if (result.color == kRedTarget) { + } else if (result.color == kRedTargetRight) { returnable = "Red"; - } else if (result.color == kGreenTarget) { - returnable = "Green"; - } else if (result.color == kYellowTarget) { - returnable = "Yellow"; - } else { - returnable = "Unknown"; - } + } else if (result.color == kGreenTargetRight) { + returnable = "Green"; + } else if (result.color == kYellowTargetRight) { + returnable = "Yellow"; + } else { + returnable = "Unknown"; + } + + return returnable; - return returnable; - } public boolean onSameColor() { - if (getColorR() == getColorL()) { - return true; + if (getColorR() == getColorL()) { + return true; + } + return false; + } + + public static int findMatchingColorIndex(String[] arr, String key) { + for (int i = 0; i < arr.length; i++) { + if (arr[i] == key) { + return i; } - return false; + } + return arr.length + 1; } + public static String getTargetColor() { + String target = DriverStation.getInstance().getGameSpecificMessage(); + + if (target == "R" || target == "Y") { + return colors1[findMatchingColorIndex(colors1, target) - 2]; + } else if (target == "B" || target == "G"){ + return colors2[findMatchingColorIndex(colors2, target) - 2]; + } + return null; + }//returns a single character representing the color the color sensors should be + //looking for to make the field sensor sense the proper target color + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/ClimbSub.java b/src/main/java/frc/robot/subsystems/ClimbSub.java index 9a9b9c8..3e375bd 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSub.java +++ b/src/main/java/frc/robot/subsystems/ClimbSub.java @@ -12,23 +12,23 @@ public ClimbSub() { } public void lift(double speed){ - RobotContainer.lift.set(speed); + // RobotContainer.lift.set(speed); } public void autoLift(double count, boolean up){ - SmartDashboard.putBoolean("lift condition for up", RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -count); - SmartDashboard.putBoolean("this is up", up); - - if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -count && up){ - System.out.println("climb up"); - RobotContainer.lift.set(-.3); - }else if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() < -count && !up){ - System.out.println("climb down"); - RobotContainer.lift.set(.2); - }else{ - System.out.println("climb no"); - RobotContainer.lift.set(0); - } + // SmartDashboard.putBoolean("lift condition for up", RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -count); + // SmartDashboard.putBoolean("this is up", up); + + // if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -count && up){ + // System.out.println("climb up"); + // RobotContainer.lift.set(-.3); + // }else if(RobotContainer.lift.getSensorCollection().getQuadraturePosition() < -count && !up){ + // System.out.println("climb down"); + // RobotContainer.lift.set(.2); + // }else{ + // System.out.println("climb no"); + // RobotContainer.lift.set(0); + // } } public void autoWinch(){ @@ -43,7 +43,7 @@ public void testLift(){ //all locks engaged // - if(RobotContainer.tbumperLeft.get()){ + if(RobotContainer.ta.get()){ RobotContainer.winch1.set(.3); //down positive RobotContainer.winch2.set(-.3); //down negative }else { @@ -51,13 +51,13 @@ public void testLift(){ RobotContainer.winch2.set(0); } - if(RobotContainer.mbumperLeft.get()){//down - RobotContainer.lift.set(.3); - }else if(RobotContainer.mbumperRight.get() && RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14500){ - RobotContainer.lift.set(-.3); - }else{ - RobotContainer.lift.set(0); - } + // if(RobotContainer.tbumperLeft.get()){//down + // RobotContainer.lift.set(.3); + // }else if(RobotContainer.tbumperRight.get() && RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14500){ // && RobotContainer.lift.getSensorCollection().getQuadraturePosition() > -14500 + // RobotContainer.lift.set(-.3); + // }else{ + // RobotContainer.lift.set(0); + // } if(RobotContainer.tback.get()){ lockClimb(); diff --git a/src/main/java/frc/robot/subsystems/DriveSub.java b/src/main/java/frc/robot/subsystems/DriveSub.java index d9a6604..c37d8b8 100644 --- a/src/main/java/frc/robot/subsystems/DriveSub.java +++ b/src/main/java/frc/robot/subsystems/DriveSub.java @@ -1,4 +1,3 @@ - package frc.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; @@ -25,7 +24,8 @@ public class DriveSub extends SubsystemBase { public static final double ratioGear2 = 6; public static final double eTicksPerRev = 42; //ticks of - public static final double wheelDiameter = 0.1524; //in meters + // public static final double wheelDiameter = 0.1524; //in meters + public double reverseDrive = 1; public DriveSub() { m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); @@ -36,40 +36,65 @@ public DriveSub() { } public double getAverageEncoderDistance(){ - if(RobotContainer.dsL.get() == Value.kForward){ + // if(RobotContainer.dsL.get() == Value.kForward){ + // return ((RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters + // + (-RobotContainer.r2.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters) / 2.0; + // }else{ + // return ((RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters + // + (-RobotContainer.r2.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters) / 2.0; + // } + if(RobotContainer.dsL.get() == Value.kReverse){ return ((RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters + (-RobotContainer.r2.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters) / 2.0; }else{ - return ((RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters - + (-RobotContainer.r2.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters) / 2.0; + return ((RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters + + (-RobotContainer.r2.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters) / 2.0; } } public double getTurnRate(){ - return gyroReversed*RobotContainer.navX.getRate(); + return -gyroReversed*RobotContainer.navX.getRate(); + } + + public void resetOdometry(){ + resetOdometry(new Pose2d()); } public void teleDrive(){ - if(RobotContainer.dback.get()){//high - RobotContainer.dsL.set(Value.kForward); - }else if(RobotContainer.dstart.get()){//low - RobotContainer.dsL.set(Value.kReverse); + // if(RobotContainer.dback.get()){//high + // RobotContainer.dsL.set(Value.kForward); + // }else if(RobotContainer.dstart.get()){//low + // RobotContainer.dsL.set(Value.kReverse); + // } + if((RobotContainer.driver.getRawAxis(2) > 0.5) && (RobotContainer.driver.getRawAxis(3) > 0.5)){ + reverseDrive = -1; + }else{ + reverseDrive = 1; } if(RobotContainer.dbumperLeft.get()){ - RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1) * 0.5, (-RobotContainer.driver.getRawAxis(4) * 0.5)); - } else { - RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1), -RobotContainer.driver.getRawAxis(4) * 0.8); + RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1) * 0.5 * reverseDrive, (RobotContainer.driver.getRawAxis(4) * 0.5)); + }else if(RobotContainer.dbumperRight.get()){ + RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1) * reverseDrive, (RobotContainer.driver.getRawAxis(4))); + }else{ + RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1) * reverseDrive, RobotContainer.driver.getRawAxis(4) * 0.8); } - -} + } public void drive(double xSpeed, double rotation){ RobotContainer.diffDrive.arcadeDrive(xSpeed, rotation); } public DifferentialDriveWheelSpeeds getWheelSpeeds() { + // if(DriveConstants.reverse){ + // return new DifferentialDriveWheelSpeeds( + // ((RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters, // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, + // ((-RobotContainer.l1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); + // } + // return new DifferentialDriveWheelSpeeds( + // ((RobotContainer.l1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters, // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, + // ((-RobotContainer.r1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters); //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); if(DriveConstants.reverse){ return new DifferentialDriveWheelSpeeds( ((RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters, // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, @@ -86,16 +111,31 @@ public double getWheelVelocity() { (((RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, + ((-RobotContainer.l1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters)/2; //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); } - return - (((RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, - + ((-RobotContainer.l1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters)/2; //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); - } + return + (((RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, + + ((-RobotContainer.l1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters)/2; //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); + // if(DriveConstants.reverse){ + // return + // (((-RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, + // + ((-RobotContainer.l1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters)/2; //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); + // } + // return + // (((-RobotContainer.r1.getEncoder().getVelocity()/60) / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters // (RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh) ) * Constants.wheelCircumferenceMeters, + // + ((-RobotContainer.l1.getEncoder().getVelocity()/60)/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters)/2; //(RobotContainer.dsL.get() == Value.kForward ? Constants.ticksPerRevolutionLow : Constants.ticksPerRevolutionHigh)) * Constants.wheelCircumferenceMeters); + + } + + public double getRotationSpeed(){ + if(RobotContainer.dsL.get() == Value.kReverse){ + return .5*Math.PI*(Math.abs(RobotContainer.r1.getEncoder().getVelocity()/ratioGear2)+Math.abs(RobotContainer.l1.getEncoder().getVelocity()/ratioGear2))/2/60; + }return .5*Math.PI*(Math.abs(RobotContainer.r1.getEncoder().getVelocity()/ratioGear1)+Math.abs(RobotContainer.l1.getEncoder().getVelocity()/ratioGear1))/2/60; + } double maxVolt = 12; public void tankDriveVolts(double leftVolts,double rightVolts) { if(DriveConstants.reverse){ - RobotContainer.left.setVoltage(MathUtil.clamp(-rightVolts, -maxVolt, maxVolt)); - RobotContainer.right.setVoltage(MathUtil.clamp(leftVolts, -maxVolt, maxVolt)); + RobotContainer.left.setVoltage(MathUtil.clamp(rightVolts, -maxVolt, maxVolt)); + RobotContainer.right.setVoltage(MathUtil.clamp(-leftVolts, -maxVolt, maxVolt)); }else{ RobotContainer.left.setVoltage(MathUtil.clamp(rightVolts, -maxVolt, maxVolt)); RobotContainer.right.setVoltage(MathUtil.clamp(-leftVolts, -maxVolt, maxVolt)); @@ -133,8 +173,29 @@ public void resetEncoders(){ @Override public void periodic() { + // if(DriveConstants.reverse){ + // if(RobotContainer.dsL.get() == Value.kReverse){ + // m_odometry.update(Rotation2d.fromDegrees(-getHeading()), + // (RobotContainer.r1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters, //encoder * rev/ticks = rev * circ = dist + // (-RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); + // }else{ + // m_odometry.update(Rotation2d.fromDegrees(getHeading()), + // (RobotContainer.r1.getEncoder().getPosition()/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters, + // (-RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters); + // } + // }else{ + // if(RobotContainer.dsL.get() == Value.kReverse){ + // m_odometry.update(Rotation2d.fromDegrees(getHeading()), + // (RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters, //encoder * rev/ticks = rev * circ = dist + // (-RobotContainer.r1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); + // }else{ + // m_odometry.update(Rotation2d.fromDegrees(getHeading()), + // (RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters, + // (-RobotContainer.r1.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters); + // } + // } if(DriveConstants.reverse){ - if(RobotContainer.dsL.get() == Value.kForward){ + if(RobotContainer.dsL.get() == Value.kReverse){ m_odometry.update(Rotation2d.fromDegrees(-getHeading()), (RobotContainer.r1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters, //encoder * rev/ticks = rev * circ = dist (-RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); @@ -144,7 +205,7 @@ public void periodic() { (-RobotContainer.l1.getEncoder().getPosition() / Constants.ticksPerRevolutionHigh) * Constants.wheelCircumferenceMeters); } }else{ - if(RobotContainer.dsL.get() == Value.kForward){ + if(RobotContainer.dsL.get() == Value.kReverse){ m_odometry.update(Rotation2d.fromDegrees(getHeading()), (RobotContainer.l1.getEncoder().getPosition()/ Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters, //encoder * rev/ticks = rev * circ = dist (-RobotContainer.r1.getEncoder().getPosition() / Constants.ticksPerRevolutionLow) * Constants.wheelCircumferenceMeters); @@ -156,13 +217,13 @@ public void periodic() { } } - public void shift() { - if (RobotContainer.dsL.get() == Value.kForward) { - RobotContainer.dsL.set(Value.kReverse); - } else if(RobotContainer.dsL.get() == Value.kReverse) { - RobotContainer.dsL.set(Value.kForward); - } - } + // public void shift() { + // if (RobotContainer.dsL.get() == Value.kForward) { + // RobotContainer.dsL.set(Value.kReverse); + // } else if(RobotContainer.dsL.get() == Value.kReverse) { + // RobotContainer.dsL.set(Value.kForward); + // } + // } public int getCurrentGear() { if (RobotContainer.dsL.get() == Value.kForward) { diff --git a/src/main/java/frc/robot/subsystems/FlywheelSub.java b/src/main/java/frc/robot/subsystems/FlywheelSub.java index 8c075cd..60b35e2 100644 --- a/src/main/java/frc/robot/subsystems/FlywheelSub.java +++ b/src/main/java/frc/robot/subsystems/FlywheelSub.java @@ -5,8 +5,7 @@ public class FlywheelSub extends SubsystemBase { - public static final double flywheelRadius = 1; //meters - private static final double encTicksPerRot = 0; + public static final double flywheelRadius = .25;//feet public static final double flywheelSpeed = -1; public FlywheelSub() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSub.java b/src/main/java/frc/robot/subsystems/IntakeSub.java index 37e8158..e4a1a59 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSub.java +++ b/src/main/java/frc/robot/subsystems/IntakeSub.java @@ -7,15 +7,14 @@ public class IntakeSub extends SubsystemBase { public static final double intakeSpeed = 1; - public static final double starWheelAndCPMSpeed = 0.6; + public static final double starWheelAndCPMSpeed = 0.8; public static final double conveyorMotorSpeed = -1; public static int targetTS; //which touch sensor are we looking for? public static boolean cyclingBall; - public static boolean starWheelOff = false; public IntakeSub() { - targetTS = 2; + targetTS = 3; cyclingBall = false; } @@ -40,11 +39,11 @@ public void intakeOut() { RobotContainer.intakePistons.set(Value.kReverse); } - public void runStarWheelAndCPM (double speed) { + public void runConveyorAndCPM (double speed) { RobotContainer.intake2.set(speed); } - public void runConveyorBelt(double speed) { + public void runTurretFeeder(double speed) { RobotContainer.intake3.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/LidarSub.java b/src/main/java/frc/robot/subsystems/LidarSub.java index 72b00c8..a57cc11 100644 --- a/src/main/java/frc/robot/subsystems/LidarSub.java +++ b/src/main/java/frc/robot/subsystems/LidarSub.java @@ -54,8 +54,8 @@ public double getDistance() { */ cm = (counter.getPeriod() * 1.0183 * 1000000.0 / 10.0) + CALIBRATION_OFFSET; - SmartDashboard.putNumber("This is is the period you know", counter.getPeriod()); - SmartDashboard.putString("This is counter", counter.toString()); + // SmartDashboard.putNumber("This is is the period you know", counter.getPeriod()); + // SmartDashboard.putString("This is counter", counter.toString()); SmartDashboard.putNumber("This is distance in cm", cm); return cm; } diff --git a/src/main/java/frc/robot/subsystems/Projectile.java b/src/main/java/frc/robot/subsystems/Projectile.java index a68586f..5145c5d 100644 --- a/src/main/java/frc/robot/subsystems/Projectile.java +++ b/src/main/java/frc/robot/subsystems/Projectile.java @@ -2,12 +2,9 @@ import frc.robot.RobotContainer; -/** - * Add your docs here. - */ public class Projectile { - private static double hoodHeight = 2; + private static double hoodHeight = 1.6875; private static double outerTargetHeight = 8.1875; private static double gravity = -32.2; @@ -15,12 +12,17 @@ public static double getProjectileVelocity(){ double d = RobotContainer.lidarSub.getDistance(); return Math.sqrt((-(Math.pow(d, 2) * (2 * gravity * (outerTargetHeight - hoodHeight))) / Math.pow((outerTargetHeight - hoodHeight), 2) + 2 * (gravity * Math.pow(d, 2) / 2) / (outerTargetHeight - hoodHeight) - - Math.pow(2, 2) * gravity * (outerTargetHeight - hoodHeight)) / 2); - } + - 4 * gravity * (outerTargetHeight - hoodHeight)) / 2); + } public static double getProjectileAngle(){ double velocity = getProjectileVelocity(); return Math.asin(Math.sqrt(-(2 * gravity * (outerTargetHeight - hoodHeight))/Math.pow(velocity, 2))); } + public static double experimentalMath(){ + double d = RobotContainer.lidarSub.getDistance(); + return (d * 55.6122 + 137.245); + } + } diff --git a/src/main/java/frc/robot/subsystems/TurretSub.java b/src/main/java/frc/robot/subsystems/TurretSub.java index 0802b34..588590a 100644 --- a/src/main/java/frc/robot/subsystems/TurretSub.java +++ b/src/main/java/frc/robot/subsystems/TurretSub.java @@ -9,21 +9,20 @@ public class TurretSub extends SubsystemBase { // actual //4825 tickers per revers - private static int encoderLowLimit = -8000; + private static int encoderLowLimit = 10000; private static int encoderHighLimit = 0; private static double degreesOfFreedom = 90; private static double angleAtLowLimit = -45; private static double angleAtHighLimit = 45; private static final double degreesPerTick = degreesOfFreedom / encoderHighLimit; - - public TurretSub() { } public void rotate(double speed){ - if (RobotContainer.turretMotor.getSensorCollection().getQuadraturePosition() > encoderHighLimit-100 && speed > 0){ + if (Math.abs(RobotContainer.turretMotor.getSensorCollection().getQuadraturePosition()) > encoderHighLimit-encoderHighLimit*.05 && speed < 0 || + (Math.abs(RobotContainer.turretMotor.getSensorCollection().getQuadraturePosition()) < encoderHighLimit*.05 && speed > 0)){ RobotContainer.turretMotor.set(0); } @@ -33,6 +32,7 @@ public void rotate(double speed){ // RobotContainer.turretMotor.set(0); // } else { RobotContainer.turretMotor.set(speed); + //right clockwise, left clockwise, negatived stick // } }