Started implamenting SparkMaxs and PID
This commit is contained in:
parent
d9d3d14780
commit
4ce861bda1
@ -61,15 +61,19 @@ public class RobotContainer {
|
|||||||
shooter.setDefaultCommand(shooter.stopShooter());
|
shooter.setDefaultCommand(shooter.stopShooter());
|
||||||
|
|
||||||
//controller bindings//
|
//controller bindings//
|
||||||
driver.a().onTrue(ampUp());
|
driver.a().whileTrue(ampUp());
|
||||||
driver.a().onFalse(ampScore());
|
driver.a().whileFalse(ampScore());
|
||||||
driver.leftTrigger().onTrue();
|
driver.leftTrigger().whileTrue(smartIntake());
|
||||||
|
|
||||||
|
operator.povUp().onTrue(hood.hoodUp());
|
||||||
|
operator.povDown().whileTrue(climber.runClimber());
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command getAutonomousCommand() {
|
public Command getAutonomousCommand() {
|
||||||
return Commands.print("No autonomous command configured");
|
return Commands.print("No autonomous command configured");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
//methods for smart actions//
|
//methods for smart actions//
|
||||||
public Command ampUp() {
|
public Command ampUp() {
|
||||||
if (!sensors.getHoodBeamBreak()) {
|
if (!sensors.getHoodBeamBreak()) {
|
||||||
@ -91,13 +95,13 @@ public class RobotContainer {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
public Command smartIntake() {
|
public Command smartIntake() {
|
||||||
if (!sensors.getIndexerBeamBreak()) {
|
if (!sensors.getIndexerBeamBreak()) {
|
||||||
return intakePivot.intakeDown()
|
return intakePivot.intakeDown()
|
||||||
.alongWith(null)
|
.alongWith(intakeRollers.runIntakeRollers(1));
|
||||||
} else {
|
} else {
|
||||||
|
return intakePivot.intakeUp()
|
||||||
|
.alongWith(intakeRollers.stopIntakeRollers());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
6
src/main/java/frc/robot/constants/ClimberConstants.java
Normal file
6
src/main/java/frc/robot/constants/ClimberConstants.java
Normal file
@ -0,0 +1,6 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class ClimberConstants {
|
||||||
|
public static final int kMotor1 = 0;
|
||||||
|
public static final int kMotor2 = 1;
|
||||||
|
}
|
5
src/main/java/frc/robot/constants/HoodConstants.java
Normal file
5
src/main/java/frc/robot/constants/HoodConstants.java
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class HoodConstants {
|
||||||
|
|
||||||
|
}
|
5
src/main/java/frc/robot/constants/IndexerConstants.java
Normal file
5
src/main/java/frc/robot/constants/IndexerConstants.java
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class IndexerConstants {
|
||||||
|
|
||||||
|
}
|
16
src/main/java/frc/robot/constants/IntakePivotConstants.java
Normal file
16
src/main/java/frc/robot/constants/IntakePivotConstants.java
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class IntakePivotConstants {
|
||||||
|
public static final int kUpAngle = 90;
|
||||||
|
public static final int kDownAngle = 0;
|
||||||
|
|
||||||
|
public static final int kFFS = 0;
|
||||||
|
public static final int kFFG = 0;
|
||||||
|
public static final int kFFV = 0;
|
||||||
|
|
||||||
|
public static final int kPPivot = 0;
|
||||||
|
public static final int kIPivot = 0;
|
||||||
|
public static final int kDPivot = 0;
|
||||||
|
|
||||||
|
public static final int kIntakePivotConversionFactor = 0;
|
||||||
|
}
|
@ -0,0 +1,5 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class IntakeRollersConstants {
|
||||||
|
public static final int kIntakeRoller = 0;
|
||||||
|
}
|
6
src/main/java/frc/robot/constants/SensorsConstants.java
Normal file
6
src/main/java/frc/robot/constants/SensorsConstants.java
Normal file
@ -0,0 +1,6 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class SensorsConstants {
|
||||||
|
public static final int kHoodBeamBreak = 0;
|
||||||
|
public static final int kIndexerBeamBreak = 1;
|
||||||
|
}
|
5
src/main/java/frc/robot/constants/ShooterConstants.java
Normal file
5
src/main/java/frc/robot/constants/ShooterConstants.java
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
public class ShooterConstants {
|
||||||
|
|
||||||
|
}
|
@ -1,19 +1,21 @@
|
|||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
|
import com.revrobotics.CANSparkMax;
|
||||||
|
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
import edu.wpi.first.wpilibj.DigitalInput;
|
||||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
public class Hood extends SubsystemBase {
|
public class Hood extends SubsystemBase {
|
||||||
|
|
||||||
private VictorSP pivotMotor;
|
private CANSparkMax pivotMotor;
|
||||||
|
|
||||||
private DigitalInput bottomLimitSwitch;
|
private DigitalInput bottomLimitSwitch;
|
||||||
private DigitalInput topLimitSwitch;
|
private DigitalInput topLimitSwitch;
|
||||||
|
|
||||||
public Hood() {
|
public Hood() {
|
||||||
pivotMotor = new VictorSP(0);
|
pivotMotor = new CANSparkMax(0, MotorType.kBrushless);
|
||||||
|
|
||||||
bottomLimitSwitch = new DigitalInput(0);
|
bottomLimitSwitch = new DigitalInput(0);
|
||||||
topLimitSwitch = new DigitalInput(0);
|
topLimitSwitch = new DigitalInput(0);
|
||||||
|
@ -17,12 +17,6 @@ public class Indexer extends SubsystemBase {
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command reverseIndexer(double speed) {
|
|
||||||
return run(() -> {
|
|
||||||
motor1.set(speed);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command stopIndexer() {
|
public Command stopIndexer() {
|
||||||
return run(() -> {
|
return run(() -> {
|
||||||
motor1.set(0);
|
motor1.set(0);
|
||||||
|
@ -1,40 +1,74 @@
|
|||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
import com.revrobotics.CANSparkMax;
|
||||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
import com.revrobotics.RelativeEncoder;
|
||||||
|
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.controller.ArmFeedforward;
|
||||||
|
import edu.wpi.first.math.controller.PIDController;
|
||||||
|
import edu.wpi.first.math.util.Units;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
import frc.robot.constants.IntakePivotConstants;
|
||||||
|
|
||||||
public class IntakePivot extends SubsystemBase {
|
public class IntakePivot extends SubsystemBase {
|
||||||
private VictorSP pivotMotor;
|
private CANSparkMax pivotMotor;
|
||||||
|
|
||||||
private DigitalInput topLimitSwitch;
|
private RelativeEncoder intakeEncoder;
|
||||||
private DigitalInput bottomLimitSwitch;
|
|
||||||
|
private PIDController intakePIDController;
|
||||||
|
private ArmFeedforward intakeFF;
|
||||||
|
|
||||||
public IntakePivot() {
|
public IntakePivot() {
|
||||||
pivotMotor = new VictorSP(0);
|
pivotMotor = new CANSparkMax(0, MotorType.kBrushless);
|
||||||
|
|
||||||
topLimitSwitch = new DigitalInput(0);
|
intakeEncoder = pivotMotor.getEncoder();
|
||||||
bottomLimitSwitch = new DigitalInput(0);
|
|
||||||
|
intakeFF = new ArmFeedforward(
|
||||||
|
IntakePivotConstants.kFFS,
|
||||||
|
IntakePivotConstants.kFFG,
|
||||||
|
IntakePivotConstants.kFFV
|
||||||
|
);
|
||||||
|
|
||||||
|
intakePIDController = new PIDController(
|
||||||
|
IntakePivotConstants.kPPivot,
|
||||||
|
IntakePivotConstants.kIPivot,
|
||||||
|
IntakePivotConstants.kDPivot
|
||||||
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command intakeUp() {
|
public Command intakeUp() {
|
||||||
return run(() -> {
|
return run(() -> {
|
||||||
if (topLimitSwitch.get()) {
|
pivotMotor.setVoltage(
|
||||||
pivotMotor.set(0);
|
intakePIDController.calculate(
|
||||||
} else {
|
getIntakeAngle(),
|
||||||
pivotMotor.set(0.5);
|
IntakePivotConstants.kUpAngle
|
||||||
}
|
) + intakeFF.calculate(
|
||||||
|
IntakePivotConstants.kUpAngle,
|
||||||
|
0.0
|
||||||
|
)
|
||||||
|
);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command intakeDown() {
|
public Command intakeDown() {
|
||||||
return run(() -> {
|
return run(() -> {
|
||||||
if (bottomLimitSwitch.get()) {
|
pivotMotor.setVoltage(
|
||||||
pivotMotor.set(0);
|
intakePIDController.calculate(
|
||||||
} else {
|
getIntakeAngle(),
|
||||||
pivotMotor.set(-0.5);
|
IntakePivotConstants.kDownAngle
|
||||||
}
|
) + intakeFF.calculate(
|
||||||
|
IntakePivotConstants.kDownAngle,
|
||||||
|
0.0
|
||||||
|
)
|
||||||
|
);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public double getIntakeAngle(){
|
||||||
|
return Units.rotationsToRadians(
|
||||||
|
intakeEncoder.getPosition()/
|
||||||
|
IntakePivotConstants.kIntakePivotConversionFactor
|
||||||
|
);
|
||||||
|
}
|
||||||
}
|
}
|
@ -1,14 +1,16 @@
|
|||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
import com.revrobotics.CANSparkMax;
|
||||||
|
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
public class IntakeRollers extends SubsystemBase {
|
public class IntakeRollers extends SubsystemBase {
|
||||||
private VictorSP motor1;
|
private CANSparkMax motor1;
|
||||||
|
|
||||||
public IntakeRollers() {
|
public IntakeRollers() {
|
||||||
motor1 = new VictorSP(0);
|
motor1 = new CANSparkMax(0, MotorType.kBrushless);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command runIntakeRollers(double speed) {
|
public Command runIntakeRollers(double speed) {
|
||||||
@ -17,12 +19,6 @@ public class IntakeRollers extends SubsystemBase {
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command reverseIntakeRollers(double speed) {
|
|
||||||
return run(() -> {
|
|
||||||
motor1.set(speed);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command stopIntakeRollers() {
|
public Command stopIntakeRollers() {
|
||||||
return run(() -> {
|
return run(() -> {
|
||||||
motor1.set(0);
|
motor1.set(0);
|
||||||
|
@ -1,18 +1,20 @@
|
|||||||
package frc.robot.subsystems;
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
import com.revrobotics.CANSparkMax;
|
||||||
|
import com.revrobotics.CANSparkLowLevel.MotorType;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
public class Shooter extends SubsystemBase {
|
public class Shooter extends SubsystemBase {
|
||||||
private VictorSP motor1;
|
private CANSparkMax motor1;
|
||||||
private VictorSP motor2;
|
private CANSparkMax motor2;
|
||||||
|
|
||||||
public Shooter() {
|
public Shooter() {
|
||||||
motor1 = new VictorSP(0);
|
motor1 = new CANSparkMax(0, MotorType.kBrushless);
|
||||||
motor2 = new VictorSP(0);
|
motor2 = new CANSparkMax(0, MotorType.kBrushless);
|
||||||
|
|
||||||
motor1.addFollower(motor2);
|
motor2.follow(motor1);
|
||||||
motor2.setInverted(true);
|
motor2.setInverted(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -22,12 +24,6 @@ public class Shooter extends SubsystemBase {
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command reverseShooter() {
|
|
||||||
return run(() -> {
|
|
||||||
motor1.set(-1);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
public Command stopShooter() {
|
public Command stopShooter() {
|
||||||
return run(() -> {
|
return run(() -> {
|
||||||
motor1.set(0);
|
motor1.set(0);
|
||||||
|
74
vendordeps/REVLib.json
Normal file
74
vendordeps/REVLib.json
Normal file
@ -0,0 +1,74 @@
|
|||||||
|
{
|
||||||
|
"fileName": "REVLib.json",
|
||||||
|
"name": "REVLib",
|
||||||
|
"version": "2024.2.4",
|
||||||
|
"frcYear": "2024",
|
||||||
|
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
|
||||||
|
"mavenUrls": [
|
||||||
|
"https://maven.revrobotics.com/"
|
||||||
|
],
|
||||||
|
"jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
|
||||||
|
"javaDependencies": [
|
||||||
|
{
|
||||||
|
"groupId": "com.revrobotics.frc",
|
||||||
|
"artifactId": "REVLib-java",
|
||||||
|
"version": "2024.2.4"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"jniDependencies": [
|
||||||
|
{
|
||||||
|
"groupId": "com.revrobotics.frc",
|
||||||
|
"artifactId": "REVLib-driver",
|
||||||
|
"version": "2024.2.4",
|
||||||
|
"skipInvalidPlatforms": true,
|
||||||
|
"isJar": false,
|
||||||
|
"validPlatforms": [
|
||||||
|
"windowsx86-64",
|
||||||
|
"windowsx86",
|
||||||
|
"linuxarm64",
|
||||||
|
"linuxx86-64",
|
||||||
|
"linuxathena",
|
||||||
|
"linuxarm32",
|
||||||
|
"osxuniversal"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"cppDependencies": [
|
||||||
|
{
|
||||||
|
"groupId": "com.revrobotics.frc",
|
||||||
|
"artifactId": "REVLib-cpp",
|
||||||
|
"version": "2024.2.4",
|
||||||
|
"libName": "REVLib",
|
||||||
|
"headerClassifier": "headers",
|
||||||
|
"sharedLibrary": false,
|
||||||
|
"skipInvalidPlatforms": true,
|
||||||
|
"binaryPlatforms": [
|
||||||
|
"windowsx86-64",
|
||||||
|
"windowsx86",
|
||||||
|
"linuxarm64",
|
||||||
|
"linuxx86-64",
|
||||||
|
"linuxathena",
|
||||||
|
"linuxarm32",
|
||||||
|
"osxuniversal"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"groupId": "com.revrobotics.frc",
|
||||||
|
"artifactId": "REVLib-driver",
|
||||||
|
"version": "2024.2.4",
|
||||||
|
"libName": "REVLibDriver",
|
||||||
|
"headerClassifier": "headers",
|
||||||
|
"sharedLibrary": false,
|
||||||
|
"skipInvalidPlatforms": true,
|
||||||
|
"binaryPlatforms": [
|
||||||
|
"windowsx86-64",
|
||||||
|
"windowsx86",
|
||||||
|
"linuxarm64",
|
||||||
|
"linuxx86-64",
|
||||||
|
"linuxathena",
|
||||||
|
"linuxarm32",
|
||||||
|
"osxuniversal"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
Loading…
Reference in New Issue
Block a user