First subsystems added
This commit is contained in:
parent
e9febc55b7
commit
3f3cb14529
@ -0,0 +1,8 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class ClimberPivotConstants {
|
||||
public static final int kPivotMotorID = 0;
|
||||
public static final double kPIDControllerP = 0;
|
||||
public static final double kPIDControllerI = 0;
|
||||
public static final double kPIDControllerD = 0;
|
||||
}
|
@ -0,0 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class ClimberRollersConstants {
|
||||
public static final int kRollerMotorID = 0;
|
||||
}
|
13
src/main/java/frc/robot/constants/ElevatorConstants.java
Normal file
13
src/main/java/frc/robot/constants/ElevatorConstants.java
Normal file
@ -0,0 +1,13 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class ElevatorConstants {
|
||||
public static final int kMotor1ID = 0;
|
||||
|
||||
public static final double kPIDControllerP = 0;
|
||||
public static final double kPIDControllerI = 0;
|
||||
public static final double kPIDControllerD = 0;
|
||||
|
||||
public static final double kFeedForwardS = 0;
|
||||
public static final double kFeedForwardG = 0;
|
||||
public static final double kFeedForwardV = 0;
|
||||
}
|
@ -0,0 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class ManipulatorConstants {
|
||||
|
||||
}
|
5
src/main/java/frc/robot/constants/SensorsConstants.java
Normal file
5
src/main/java/frc/robot/constants/SensorsConstants.java
Normal file
@ -0,0 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class SensorsConstants {
|
||||
public static final int kClimberLimitSwitchID = 0;
|
||||
}
|
51
src/main/java/frc/robot/subsystems/ClimberPivot.java
Normal file
51
src/main/java/frc/robot/subsystems/ClimberPivot.java
Normal file
@ -0,0 +1,51 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.revrobotics.AbsoluteEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.ClimberPivotConstants;
|
||||
|
||||
public class ClimberPivot extends SubsystemBase {
|
||||
private SparkMax pivotMotor;
|
||||
|
||||
private AbsoluteEncoder neoEncoder;
|
||||
|
||||
private PIDController pidController;
|
||||
|
||||
public ClimberPivot() {
|
||||
pivotMotor = new SparkMax(
|
||||
ClimberPivotConstants.kPivotMotorID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
pivotMotor.setInverted(true);
|
||||
|
||||
neoEncoder = pivotMotor.getAbsoluteEncoder();
|
||||
|
||||
pidController = new PIDController(
|
||||
ClimberPivotConstants.kPIDControllerP,
|
||||
ClimberPivotConstants.kPIDControllerI,
|
||||
ClimberPivotConstants.kPIDControllerD
|
||||
);
|
||||
}
|
||||
|
||||
public Command runPivot(double speed) {
|
||||
return run(() -> {
|
||||
pivotMotor.set(speed);
|
||||
});
|
||||
}
|
||||
|
||||
public Command goToAngle(double setpoint) {
|
||||
return run(() -> {
|
||||
pivotMotor.set(
|
||||
pidController.calculate(
|
||||
neoEncoder.getPosition(),
|
||||
setpoint
|
||||
)
|
||||
);
|
||||
});
|
||||
}
|
||||
}
|
26
src/main/java/frc/robot/subsystems/ClimberRollers.java
Normal file
26
src/main/java/frc/robot/subsystems/ClimberRollers.java
Normal file
@ -0,0 +1,26 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.ClimberRollersConstants;
|
||||
|
||||
public class ClimberRollers extends SubsystemBase {
|
||||
private SparkMax rollerMotor;
|
||||
|
||||
public ClimberRollers() {
|
||||
rollerMotor = new SparkMax(
|
||||
ClimberRollersConstants.kRollerMotorID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
rollerMotor.setInverted(false);
|
||||
}
|
||||
|
||||
public Command runRoller(double speed) {
|
||||
return run(() -> {
|
||||
rollerMotor.set(speed);
|
||||
});
|
||||
}
|
||||
}
|
60
src/main/java/frc/robot/subsystems/Elevator.java
Normal file
60
src/main/java/frc/robot/subsystems/Elevator.java
Normal file
@ -0,0 +1,60 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.revrobotics.AbsoluteEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.math.controller.ElevatorFeedforward;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
|
||||
public class Elevator extends SubsystemBase {
|
||||
private SparkMax motor1;
|
||||
|
||||
private AbsoluteEncoder encoder;
|
||||
|
||||
private PIDController pidController;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
|
||||
public Elevator() {
|
||||
motor1 = new SparkMax(
|
||||
ElevatorConstants.kMotor1ID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
|
||||
encoder = motor1.getAbsoluteEncoder();
|
||||
|
||||
pidController = new PIDController(
|
||||
ElevatorConstants.kPIDControllerP,
|
||||
ElevatorConstants.kPIDControllerI,
|
||||
ElevatorConstants.kPIDControllerD
|
||||
);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
ElevatorConstants.kFeedForwardG,
|
||||
ElevatorConstants.kFeedForwardV
|
||||
);
|
||||
}
|
||||
|
||||
public Command runElevator(double speed) {
|
||||
return run(() -> {
|
||||
motor1.set(speed);
|
||||
});
|
||||
}
|
||||
|
||||
public Command goToSetpoint(double setpoint) {
|
||||
return run(() -> {
|
||||
motor1.set(feedForward.calculate(
|
||||
encoder.getVelocity(),
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint
|
||||
)
|
||||
));
|
||||
});
|
||||
}
|
||||
}
|
17
src/main/java/frc/robot/subsystems/Sensors.java
Normal file
17
src/main/java/frc/robot/subsystems/Sensors.java
Normal file
@ -0,0 +1,17 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.SensorsConstants;
|
||||
|
||||
public class Sensors extends SubsystemBase {
|
||||
private DigitalInput climberLimitSwitch;
|
||||
|
||||
public Sensors() {
|
||||
climberLimitSwitch = new DigitalInput(SensorsConstants.kClimberLimitSwitchID);
|
||||
}
|
||||
|
||||
public boolean getClimberLimitSwitch() {
|
||||
return climberLimitSwitch.get();
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user