Skip to content

Commit

Permalink
Add preliminary intake
Browse files Browse the repository at this point in the history
  • Loading branch information
rachitkakkar committed Jan 10, 2024
1 parent 9e67a07 commit 97571fa
Show file tree
Hide file tree
Showing 2 changed files with 78 additions and 0 deletions.
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,10 @@ public static class VisionHardware {
public static final Rotation2d CAMERA_C_FOV = Rotation2d.fromDegrees(79.7);
}

public static class IntakeHardware {
public static final Spark.ID ROLLER_MOTOR_ID = new Spark.ID("IntakeHardware/LeftFront/Drive", 10);
}

public static class SmartDashboard {
public static final String SMARTDASHBOARD_DEFAULT_TAB = "SmartDashboard";
public static final String SMARTDASHBOARD_AUTO_MODE = "Auto Mode";
Expand Down
74 changes: 74 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
// 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.subsystems.intake;

import java.util.function.DoubleSupplier;

import org.lasarobotics.hardware.revrobotics.Spark;
import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;

import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.SparkPIDController.ArbFFUnits;

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

public class IntakeSubsystem extends SubsystemBase {
public static class Hardware {
private Spark rollorMotor;

public Hardware(Spark rollorMotor) {
this.rollorMotor = rollorMotor;
}
}

private Spark m_rollorMotor;

/** Creates a new IntakeSubsystem. */
public IntakeSubsystem(Hardware intakeHardware) {
this.m_rollorMotor = intakeHardware.rollorMotor;
}

/**
* Initialize hardware devices for intake subsystem
*
* @return Hardware object containing all necessary devices for this subsystem
*/
public static Hardware initializeHardware() {
Hardware intakeHardware = new Hardware(
new Spark(Constants.IntakeHardware.ROLLER_MOTOR_ID, MotorKind.NEO)
);
return intakeHardware;
}

// Tells the robot to intake
private void intake(double speed) {
m_rollorMotor.set(speed, ControlType.kDutyCycle, 0.0, ArbFFUnits.kPercentOut);
}

public Command intakeCommand(DoubleSupplier speed) {
return startEnd(() -> intake(speed.getAsDouble()), () -> stop());
}

// Tells the robot to outtake
private void outtake(double speed) {
m_rollorMotor.set(-speed, ControlType.kDutyCycle, 0.0, ArbFFUnits.kPercentOut);
}

public Command outtakeCommand(DoubleSupplier speed) {
return startEnd(() -> outtake(speed.getAsDouble()), () -> stop());
}

// Stop the robot
private void stop() {
m_rollorMotor.stopMotor();;
}

@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

0 comments on commit 97571fa

Please sign in to comment.