Skip to content

Commit

Permalink
chore: sent stuff to constants
Browse files Browse the repository at this point in the history
  • Loading branch information
ProgrammingSR committed Feb 17, 2024
1 parent 81246ca commit e290e26
Show file tree
Hide file tree
Showing 5 changed files with 61 additions and 29 deletions.
33 changes: 33 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

public final class Constants {


//Intake PID and Encoder Constants
public static class IntakeConstants {
public static final double kIntakeDroppedAngle = 9.0;
public static final double kIntakeRaisedAngle = 9.0;
public static final int kIntakeMotorID = 0;
public static final int kArmMotorID = 0;
public static final double kIntakeP = 0;
public static final double kIntakeI = 0;
public static final double kIntakeD = 0;
public static final double kArmP = 0;
public static final double kArmI = 0;
public static final double kArmD = 0;
public static final int kArmEncoderCh = 0;
}

//Shooter subsystem speed constants
public static class ShooterConstants {
public static final double kSpinSpeedTrue = 0.75;
public static final double kSpinSpeedFalse = 0;


}
}

6 changes: 0 additions & 6 deletions src/main/java/frc/robot/IntakeConstants.java

This file was deleted.

20 changes: 15 additions & 5 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,13 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants.IntakeConstants;
import frc.robot.Constants.ShooterConstants;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;

public class RobotContainer {
//The robots subsystems are defined here
public final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
public final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
private boolean IntakeDropped = false;
Expand All @@ -22,9 +25,13 @@ public class RobotContainer {
private XboxController controller = new XboxController(0);
public RobotContainer() {
configureBindings();
m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),m_IntakeSubsystem));
m_IntakeSubsystem.setDefaultCommand(new InstantCommand(()-> m_IntakeSubsystem.load(controller.getRightTriggerAxis()-controller.getLeftTriggerAxis(),IntakeDropped ? IntakeConstants.kIntakeDroppedAngle : IntakeConstants.kIntakeRaisedAngle),m_IntakeSubsystem));
}


/**
* checks for intake button on controller every tick
*/
public void periodic(){
if(controller.getAButton()){
lastAButton = true;
Expand All @@ -36,13 +43,16 @@ public void periodic(){
}
}

/**
* Use this method to define your button->command mappings.
*/
private void configureBindings() {
new JoystickButton(controller, Button.kB.value)
.onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(0.75), m_ShooterSubsystem))
.onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem));
.onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedTrue), m_ShooterSubsystem))
.onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedFalse), m_ShooterSubsystem));
new JoystickButton(controller, Button.kA.value)
.onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-0.75), m_ShooterSubsystem))
.onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(0), m_ShooterSubsystem));
.onTrue(new InstantCommand(() -> m_ShooterSubsystem.spin(-ShooterConstants.kSpinSpeedTrue), m_ShooterSubsystem))
.onFalse(new InstantCommand(() -> m_ShooterSubsystem.spin(ShooterConstants.kSpinSpeedFalse), m_ShooterSubsystem));
}

public Command getAutonomousCommand() {
Expand Down
30 changes: 12 additions & 18 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,21 +11,17 @@
import edu.wpi.first.wpilibj.DutyCycleEncoder;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.IntakeConstants;

//creates new motors and pid controllers lmao
public class IntakeSubsystem extends SubsystemBase {
//TODO: add constants
CANSparkFlex intakeMotor = new CANSparkFlex(0, MotorType.kBrushless);
CANSparkFlex armMotor = new CANSparkFlex(0, MotorType.kBrushless);
CANSparkFlex intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
CANSparkFlex armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless);


PIDController intakeVeloPID = new PIDController(0,0,0);
PIDController armPID = new PIDController(0,0,0);
DutyCycleEncoder armEncoder = new DutyCycleEncoder(0);
PIDController intakeVeloPID = new PIDController(IntakeConstants.kIntakeP,IntakeConstants.kIntakeI,IntakeConstants.kIntakeD);
PIDController armPID = new PIDController(IntakeConstants.kArmP,IntakeConstants.kArmI,IntakeConstants.kArmD);
DutyCycleEncoder armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh);





/** Creates a new intake. */
public IntakeSubsystem() {

Expand All @@ -41,19 +37,17 @@ public void load(double speed, double angle){
tiltToAngle(angle);

}
/**
*
*@param angle in radians
*@everyone join vc we are playing gartic phone
*/

/**
*
* @param angle motor to apply to intake
*
*/
public void tiltToAngle(double angle) {
double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle);
armMotor.set(motorPower);
}



@Override
public void periodic() {
// This method will be called once per scheduler run
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class ShooterSubsystem extends SubsystemBase {
/** Creates a new ShooterSubsystem. */
CANSparkFlex m_bottom = new CANSparkFlex(20, MotorType.kBrushless);
Expand Down

0 comments on commit e290e26

Please sign in to comment.