diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cb077a8..d89d249 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +//import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import frc.robot.subsystems.colorWheel_subsystem; import frc.robot.subsystems.drive_subsystem; import frc.robot.subsystems.index_subsystem; @@ -32,12 +32,10 @@ public class Robot extends TimedRobot { public static OI m_oi; public static AHRS navx; - private String m_autoSelected; - private final SendableChooser m_chooser = new SendableChooser<>(); + //private String m_autoSelected; + //private final SendableChooser m_chooser = new SendableChooser<>(); private static DateFormat dateFormat = new SimpleDateFormat("HH:mm:ss"); private static Date date = new Date(); - private static boolean indexInitalized; - private static boolean colorwheelInitalized; public static Encoder rightEncoder = new Encoder(RobotMap.leftEncoderPort1, RobotMap.leftEncoderPort2, false, EncodingType.k4X); public static Encoder leftEncoder = new Encoder(RobotMap.rightEncoderPort1, RobotMap.rightEncoderPort2, false, EncodingType.k4X); //declare subsystems @@ -58,9 +56,10 @@ public Robot(){ */ @Override public void robotInit() { - //index_subsystem = new index_subsystem(); + System.out.println("Made it"); + index_subsystem = new index_subsystem(); shooter_subsystem = new shooter_subsystem(); - //colorWheel_subsystem = new colorWheel_subsystem(); + colorWheel_subsystem = new colorWheel_subsystem(); drive_subsystem = new drive_subsystem(); intake_subsystem = new intake_subsystem(); m_oi = new OI(); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 226ce1c..6b4bbc0 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -32,8 +32,8 @@ public class RobotMap { //Intake Motor public static final int MOTOR_INTAKE = 6; //Shooter Motor - public static final int MOTOR_SHOOT_TOP = 9; - public static final int MOTOT_SHOOT_BOTTOM = 8; + public static final int MOTOR_SHOOT_TOP = 8; + public static final int MOTOR_SHOOT_BOTTOM = 9; //SOLENOID public static final int SOLENOID_IN = 0; public static final int SOLENOID_OUT = 1; @@ -58,8 +58,8 @@ public class RobotMap { public static final int D_PAD_UP = 6; public static final int D_PAD_DOWN = 7; public static final int D_PAD_RIGHT = 8; - public static final int START_BUTTON = 9; - public static final int SELECT_BUTTON = 10; + public static final int START_BUTTON = 8; + public static final int SELECT_BUTTON = 7; //Triggers public static final int LT = 0; diff --git a/src/main/java/frc/robot/commands/autonomousSelector.java b/src/main/java/frc/robot/commands/autonomousSelector.java index f58ca99..251bd5b 100644 --- a/src/main/java/frc/robot/commands/autonomousSelector.java +++ b/src/main/java/frc/robot/commands/autonomousSelector.java @@ -18,22 +18,24 @@ public autonomousSelector(double position, String priority) { Robot.WriteOut("Autonomous Selector Initialized"); if(position == 1){ Robot.WriteOut("Selected Postition 1"); - if(priority.equalsIgnoreCase("Mane1")){ + if(priority.equalsIgnoreCase("mane1")){ Robot.WriteOut("Selected Maneuver 1 on Position 1"); addSequential(new driveOverLine()); } - else if(priority.equalsIgnoreCase("Mane2")){ + else if(priority.equalsIgnoreCase("mane2")){ Robot.WriteOut("Selected Maneuver 2 on Position 1"); addSequential(new turnRightGoStraight()); } } else if(position == 2){ Robot.WriteOut("Selected Postition 2"); - if(priority.equalsIgnoreCase("Mane1")){ + if(priority.equalsIgnoreCase("mane1")){ Robot.WriteOut("Selected Maneuver 1 on Position 2"); + } - else if(priority.equalsIgnoreCase("Mane2")){ + else if(priority.equalsIgnoreCase("mane2")){ Robot.WriteOut("Selected Maneuver 2 on Position 2"); + } } } diff --git a/src/main/java/frc/robot/commands/colorWheel_command.java b/src/main/java/frc/robot/commands/colorWheel_command.java index 8a27542..82bc89b 100644 --- a/src/main/java/frc/robot/commands/colorWheel_command.java +++ b/src/main/java/frc/robot/commands/colorWheel_command.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; -import frc.robot.RobotMap; import java.util.ArrayList; import edu.wpi.first.wpilibj.DriverStation; @@ -48,7 +47,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (Robot.m_oi.Intake_buttonY.get() && yPressed == false) {//this is not needed if you call the command when the button is pressed + if (Robot.m_oi.Intake_startButton.get() && yPressed == false) {//this is not needed if you call the command when the button is pressed yPressed = true; initialColor = Robot.colorWheel_subsystem.getColor(); SmartDashboard.putString("FMScolor", initialColor); diff --git a/src/main/java/frc/robot/commands/index_command.java b/src/main/java/frc/robot/commands/index_command.java index 5419ecf..5876cc8 100644 --- a/src/main/java/frc/robot/commands/index_command.java +++ b/src/main/java/frc/robot/commands/index_command.java @@ -33,16 +33,16 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if(Robot.m_oi.buttonB.get()){ - System.out.println("B"); - Robot.index_subsystem.indexActivate(kForward); - } - else{ + if(Robot.m_oi.buttonB.get()){ Robot.index_subsystem.indexActivate(kReverse); } - // If the a button is pressed the boolean value is true which activates the pnuematics of the indexing system + else{ + Robot.index_subsystem.indexActivate(kForward); + }// If the a button is pressed the boolean value is true which activates the pnuematics of the indexing system + Robot.index_subsystem.checkCompressor(); } + // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/intake_command.java b/src/main/java/frc/robot/commands/intake_command.java index a801289..7d8d614 100644 --- a/src/main/java/frc/robot/commands/intake_command.java +++ b/src/main/java/frc/robot/commands/intake_command.java @@ -11,7 +11,6 @@ import frc.robot.Robot; public class intake_command extends Command { - double motorSpeed = 1.0; public intake_command() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); @@ -25,11 +24,22 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (Robot.m_oi.Intake_buttonA.get()){ - Robot.intake_subsystem.rollerSpeed(motorSpeed); + if(Robot.m_oi.Intake_buttonX.get()){ + Robot.intake_subsystem.rollerSpeed(1); + } + //Setting intake full speed durring shooter operation (check shooter_subsystem with same buttons) + else if(Robot.m_oi.Intake_buttonY.get()){ + Robot.intake_subsystem.rollerSpeed(1.0); + } + else if(Robot.m_oi.Intake_buttonB.get()){ + Robot.intake_subsystem.rollerSpeed(1.0); + } + //"Normal" buttons for intake operation + else if(Robot.m_oi.Intake_selectButton.get()){ + Robot.intake_subsystem.rollerSpeed(-1.0); } else if(Robot.m_oi.Intake_buttonB.get()){ - Robot.intake_subsystem.rollerSpeed(-motorSpeed); + Robot.intake_subsystem.rollerSpeed(-1); } else{ Robot.intake_subsystem.rollerSpeed(0); @@ -46,6 +56,7 @@ protected boolean isFinished() { @Override protected void end() { Robot.intake_subsystem.rollerSpeed(0); + //Robot.shooter_subsystem.shootAnywayBoth(0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/shooter_command.java b/src/main/java/frc/robot/commands/shooter_command.java index 207107c..db5791b 100644 --- a/src/main/java/frc/robot/commands/shooter_command.java +++ b/src/main/java/frc/robot/commands/shooter_command.java @@ -8,12 +8,15 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; public class shooter_command extends Command { // double topSpeed; double bottomSpeed; double bottomSpeedSet = 0.8; + double topSpeedSet = 0.8; + //double maxRPM = 5700; //double topSpeedSet = 0.8; double maxRPM = 5700; public shooter_command() { @@ -29,50 +32,48 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override - protected void execute() {//Different speeds are set based on what position the d-pad was pushed -/* if(Robot.m_oi.Intake_dPadLeft.get()){ - //topSpeed = 3000; - bottomSpeed = 1500; - // topSpeedSet = topSpeed/maxRPM; - bottomSpeedSet = bottomSpeed/maxRPM; + protected void execute() { + SmartDashboard.putNumber("Bottom RPM: ", Robot.shooter_subsystem.bottomEncoder.getVelocity()); + SmartDashboard.putNumber("Top RPM: ", Robot.shooter_subsystem.topEncoder.getVelocity()); + if(Robot.m_oi.Intake_RB.get()){ + Robot.shooter_subsystem.shooterSet(0, 0.4); } - else if(Robot.m_oi.Intake_dPadUp.get()){ - //topSpeed = 3750; - bottomSpeed = 3000; - //topSpeedSet = topSpeed/maxRPM; - bottomSpeedSet = bottomSpeed/maxRPM; + else if(Robot.m_oi.Intake_LB.get()){ + Robot.shooter_subsystem.shooterSet(0.4, 0); } - else if(Robot.m_oi.Intake_dPadRight.get()){ - // topSpeed = 5000; - bottomSpeed = 5000; - // topSpeedSet = topSpeed/maxRPM; - bottomSpeedSet = bottomSpeed/maxRPM; - } */ - if(Robot.m_oi.Intake_RB.get()){ - //Robot.shooter_subsystem.setShooterSpeeds(topSpeed, bottomSpeed); - Robot.shooter_subsystem.shootAnyway(bottomSpeedSet); // add abck topSpeedSet + else if(Robot.m_oi.Intake_LB.get() || Robot.m_oi.Intake_RB.get()){ + Robot.shooter_subsystem.shooterSet(0.4, 0.4); + } + else if(Robot.m_oi.Intake_buttonA.get()){ + Robot.shooter_subsystem.shooterSet(0.65, 0.65); + } + //Partly keeping intake in check (check intake_command with same buttons) + else if(Robot.m_oi.Intake_buttonY.get()){ + Robot.shooter_subsystem.shooterSet(0.075, 0.1); + } + else if(Robot.m_oi.Intake_buttonB.get()){ + Robot.shooter_subsystem.shooterSet(-0.075, -0.1); } else{ - Robot.shooter_subsystem.shootAnyway(0); // add abck topSpeedSet + Robot.shooter_subsystem.shooterSet(0, 0); } } - - // Make this return true when this Command no longer needs to run execute() @Override - protected boolean isFinished() { - return false; + protected boolean isFinished() { + return false; } - // Called once after isFinished returns true @Override protected void end() { - // Robot.shooter_subsystem.setShooterSpeeds(0, 0); - Robot.shooter_subsystem.shootAnyway(0); // add back 0 + Robot.shooter_subsystem.shooterSet(0, 0); // add back 0 } + // Make this return true when this Command no longer needs to run execute() + // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + end(); } } diff --git a/src/main/java/frc/robot/commands/turnRightGoStraight.java b/src/main/java/frc/robot/commands/turnRightGoStraight.java index 72776b2..776ce86 100644 --- a/src/main/java/frc/robot/commands/turnRightGoStraight.java +++ b/src/main/java/frc/robot/commands/turnRightGoStraight.java @@ -15,6 +15,6 @@ public class turnRightGoStraight extends CommandGroup { */ public turnRightGoStraight() { addSequential(new turnGyro_command(90, .5)); - addSequential(new driveStraight_command(0.5,100,-1)); + addSequential(new driveStraight_command(0.5, 100, -1)); } } diff --git a/src/main/java/frc/robot/subsystems/index_subsystem.java b/src/main/java/frc/robot/subsystems/index_subsystem.java index 08cc794..8c160f0 100644 --- a/src/main/java/frc/robot/subsystems/index_subsystem.java +++ b/src/main/java/frc/robot/subsystems/index_subsystem.java @@ -32,22 +32,29 @@ public void initDefaultCommand() { // setDefaultCommand(new MySpecialCommand()); } public index_subsystem(){ - airComp = new Compressor(10); - airComp.clearAllPCMStickyFaults(); - airComp.stop(); - solenoid = new DoubleSolenoid(RobotMap.SOLENOID_IN, RobotMap.SOLENOID_OUT); - if(airComp.getPressureSwitchValue()){ + airComp = new Compressor(1); + //airComp.clearAllPCMStickyFaults(); + solenoid = new DoubleSolenoid(1, RobotMap.SOLENOID_IN, RobotMap.SOLENOID_OUT); + } + + /** Turns on or off the compressor + */ + public void checkCompressor(){ + System.out.println("Switch "+ airComp.getPressureSwitchValue()); + if(!airComp.getPressureSwitchValue()){ // remember to fix this so take away the ! airComp.start(); } else{ airComp.stop(); } } - + + /** Extends or retracts both pneumatic pistons based on 'active' * @param active if true pistons are extended if false they are retracted */ public void indexActivate(Value status){ + System.out.println("Status " + status); solenoid.set(status); } diff --git a/src/main/java/frc/robot/subsystems/shooter_subsystem.java b/src/main/java/frc/robot/subsystems/shooter_subsystem.java index caa1b34..b26cce1 100644 --- a/src/main/java/frc/robot/subsystems/shooter_subsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter_subsystem.java @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ package frc.robot.subsystems; +import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; @@ -21,13 +21,10 @@ public class shooter_subsystem extends Subsystem { // here. Call these from Commands. CANPIDController bottomPID; CANPIDController topPID; + public CANEncoder topEncoder; + public CANEncoder bottomEncoder; CANSparkMax bottomMotor; - // CANSparkMax topMotor; - double kP = 0; - double kI = 0; - double kD = 0; - double kIz = 0; - double kFF = 0; + CANSparkMax topMotor; @Override public void initDefaultCommand() { setDefaultCommand(new shooter_command()); @@ -36,42 +33,19 @@ public void initDefaultCommand() { } //Declaring PID and motors for shooter public shooter_subsystem(){ - //topMotor = new CANSparkMax(RobotMap.MOTOR_SHOOT_TOP, MotorType.kBrushless); - bottomMotor = new CANSparkMax(RobotMap.MOTOT_SHOOT_BOTTOM, MotorType.kBrushless); - //topPID = topMotor.getPIDController(); - bottomPID = bottomMotor.getPIDController(); - //topPID.setReference(0, ControlType.kSmartMotion); - //bottomPID.setReference(0, ControlType.kSmartMotion); - /* topPID.setP(kP); - topPID.setI(kI); - topPID.setFF(kFF); - topPID.setIZone(kIz); - bottomPID.setP(kP); - bottomPID.setI(kI); - bottomPID.setFF(kFF); - bottomPID.setIZone(kIz); */ + topMotor = new CANSparkMax(RobotMap.MOTOR_SHOOT_TOP, MotorType.kBrushless); + bottomMotor = new CANSparkMax(RobotMap.MOTOR_SHOOT_BOTTOM, MotorType.kBrushless); + topEncoder = new CANEncoder(topMotor); + bottomEncoder = new CANEncoder(bottomMotor); + topMotor.setInverted(true); } /** Sets the shooters speeds (in RPM) - *@param topRPM the max RPM for the top of the shooter (between 5700RPM-1000RPM) - *@param bottomRPM the max RPM for the bottom of the shooter (between 5700RPM-1000RPM) + *@param topMotorSpeed the speed for the top of the shooter (-1 to 1) + *@param bottomMotorSpeed the speed for the bottom of the shooter (-1 to 1) */ - /* public void setShooterSpeeds(double topRPM, double bottomRPM){ - if(topRPM < 5700 && bottomRPM < 5700 && topRPM > 1000 && bottomRPM > 1000){ - bottomPID.setSmartMotionMaxVelocity(bottomRPM, 0); - //topPID.setSmartMotionMaxVelocity(topRPM, 0); - bottomPID.setSmartMotionMinOutputVelocity(bottomRPM-100, 0); - // topPID.setSmartMotionMinOutputVelocity(topRPM-100, 0); - bottomPID.setSmartMotionMaxAccel(1500, 0); - // topPID.setSmartMotionMaxAccel(1500, 0); - } - else{ - //System.out.println("Too fast!"); - //topMotor.stopMotor(); - bottomMotor.stopMotor(); - } */ - // } - public void shootAnyway(double bottomMotorSpeed){ // add back double topMotorSpeed - //topMotor.set(topMotorSpeed); + public void shooterSet(double topMotorSpeed, double bottomMotorSpeed){ + topMotor.set(topMotorSpeed); bottomMotor.set(bottomMotorSpeed); + } }