Skip to content

Commit

Permalink
fix/refactor: fixed intake and arm logic
Browse files Browse the repository at this point in the history
  • Loading branch information
@ProgrammingSR
ProgrammingSR committed Feb 20, 2024
1 parent b35629d commit fd6d287
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 111 deletions.
173 changes: 86 additions & 87 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,70 +14,72 @@

public final class Constants {

public static final class DriveConstants {
// TODO: set motor and encoder constants
public static final int kFrontLeftDriveMotorPort = 32;
public static final int kRearLeftDriveMotorPort = 29;
public static final int kFrontRightDriveMotorPort = 38;
public static final int kRearRightDriveMotorPort = 34;

public static final int kFrontLeftTurningMotorPort = 28;
public static final int kRearLeftTurningMotorPort = 22;
public static final int kFrontRightTurningMotorPort = 37;
public static final int kRearRightTurningMotorPort = 26;

public static final int kFrontLeftTurningEncoderPort = 19;
public static final int kRearLeftTurningEncoderPort = 20;
public static final int kFrontRightTurningEncoderPort = 18;
public static final int kRearRightTurningEncoderPort = 17;

public static final double kFrontLeftTurningEncoderOffset = 0;
public static final double kRearLeftTurningEncoderOffset = 0;
public static final double kFrontRightTurningEncoderOffset = 0;
public static final double kRearRightTurningEncoderOffset = 0;

// TODO: Test motor orientations before driving on an actual robot
public static final boolean kFrontLeftDriveMotorReversed = false;
public static final boolean kRearLeftDriveMotorReversed = false;
public static final boolean kFrontRightDriveMotorReversed = true;
public static final boolean kRearRightDriveMotorReversed = true;

/** Distance between centers of right and left wheels on robot (in meters). */
public static final double kTrackWidth = 0.57785;

/** Distance between front and back wheels on robot (in meters). */
public static final double kWheelBase = 0.57785;

/** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
public static final double kWheelDiameterMeters = 0.1;

/** Gear ratio between the motor and the wheel. */
public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
// public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration

// TODO: Tune this PID before running on a robot on the ground
public static final double kPModuleTurningController = -0.3;

public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));

/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxSpeedMetersPerSecond = 4.4196;
/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
// ^^ Calculated using the method taken from the old SDS github example

/** Heading Correction */
public static final double kHeadingCorrectionTurningStopTime = 0.2;
// TODO: Tune this PID before running on a robot on the ground
public static final double kPHeadingCorrectionController = 5;
}
//Intake PID and Encoder Constants
public static final class DriveConstants {
// TODO: set motor and encoder constants
public static final int kFrontLeftDriveMotorPort = 32;
public static final int kRearLeftDriveMotorPort = 29;
public static final int kFrontRightDriveMotorPort = 38;
public static final int kRearRightDriveMotorPort = 34;

public static final int kFrontLeftTurningMotorPort = 28;
public static final int kRearLeftTurningMotorPort = 22;
public static final int kFrontRightTurningMotorPort = 37;
public static final int kRearRightTurningMotorPort = 26;

public static final int kFrontLeftTurningEncoderPort = 19;
public static final int kRearLeftTurningEncoderPort = 20;
public static final int kFrontRightTurningEncoderPort = 18;
public static final int kRearRightTurningEncoderPort = 17;

public static final double kFrontLeftTurningEncoderOffset = 0;
public static final double kRearLeftTurningEncoderOffset = 0;
public static final double kFrontRightTurningEncoderOffset = 0;
public static final double kRearRightTurningEncoderOffset = 0;

// TODO: Test motor orientations before driving on an actual robot
public static final boolean kFrontLeftDriveMotorReversed = false;
public static final boolean kRearLeftDriveMotorReversed = false;
public static final boolean kFrontRightDriveMotorReversed = true;
public static final boolean kRearRightDriveMotorReversed = true;

/** Distance between centers of right and left wheels on robot (in meters). */
public static final double kTrackWidth = 0.57785;

/** Distance between front and back wheels on robot (in meters). */
public static final double kWheelBase = 0.57785;

/** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
public static final double kWheelDiameterMeters = 0.1;

/** Gear ratio between the motor and the wheel. */
public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
// public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2
// configuration

// TODO: Tune this PID before running on a robot on the ground
public static final double kPModuleTurningController = -0.3;

public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));

/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxSpeedMetersPerSecond = 4.4196;
/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
// ^^ Calculated using the method taken from the old SDS github example

/** Heading Correction */
public static final double kHeadingCorrectionTurningStopTime = 0.2;
// TODO: Tune this PID before running on a robot on the ground
public static final double kPHeadingCorrectionController = 5;
}

// Intake PID and Encoder Constants
public static class IntakeConstants {
public static final double kIntakeDroppedAngle = 9.0;
public static final double kIntakeLoweredAngle = 9.0;
public static final double kIntakeRaisedAngle = 9.0;
public static final int kIntakeMotorID = 0;
public static final int kArmMotorID = 0;
Expand All @@ -88,38 +90,35 @@ public static class IntakeConstants {
public static final double kArmI = 0;
public static final double kArmD = 0;
public static final int kArmEncoderCh = 0;
public static final double IntakeDroppedAngle = 0;
public static final double IntakeRaisedAngle = 0;
public static double kIntakeSpeed;
}

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

}

public static final class VisionConstants {
// TODO: Update cam pose relative to center of bot
public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
public static final double[] kLimelightCamPose = {
kCamPose.getX(),
kCamPose.getY(),
kCamPose.getZ(),
kCamPose.getRotation().getX(),
kCamPose.getRotation().getY(),
kCamPose.getRotation().getZ() };

// TODO: Experiment with different std devs in the pose estimator
public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);

// Field size in meters
public static final double kFieldWidth = 8.21055;
public static final double kFieldLength = 16.54175;
}

public static final class VisionConstants {
// TODO: Update cam pose relative to center of bot
public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
public static final double[] kLimelightCamPose = {
kCamPose.getX(),
kCamPose.getY(),
kCamPose.getZ(),
kCamPose.getRotation().getX(),
kCamPose.getRotation().getY(),
kCamPose.getRotation().getZ() };

// TODO: Experiment with different std devs in the pose estimator
public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);

// Field size in meters
public static final double kFieldWidth = 8.21055;
public static final double kFieldLength = 16.54175;
}
}

21 changes: 13 additions & 8 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,19 +29,18 @@ public class RobotContainer {
private boolean IntakeDropped = false;
private boolean lastAButton = false;

private XboxController controller = new XboxController(0);
private XboxController m_controller = new XboxController(0);

public RobotContainer() {
// Configure the trigger bindings
configureBindings();
m_intakeSubsystem.setDefaultCommand(new InstantCommand(
() -> m_intakeSubsystem.load(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis(),
IntakeDropped ? IntakeConstants.IntakeDroppedAngle : IntakeConstants.IntakeRaisedAngle),
m_intakeSubsystem));
m_intakeSubsystem.setDefaultCommand(m_controller.getLeftTriggerAxis() > 0.5
? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem)
: new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem));
}

public void periodic() {
if (controller.getAButton()) {
if (m_controller.getAButton()) {
lastAButton = true;
if (!lastAButton)
IntakeDropped = !IntakeDropped;
Expand All @@ -51,10 +50,16 @@ public void periodic() {
}

private void configureBindings() {
new JoystickButton(controller, Button.kB.value)
new JoystickButton(m_controller, Button.kY.value)
.onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), m_intakeSubsystem))
.onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
new JoystickButton(m_controller, Button.kX.value)
.onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), m_intakeSubsystem))
.onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
new JoystickButton(m_controller, Button.kB.value)
.onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
.onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
new JoystickButton(controller, Button.kA.value)
new JoystickButton(m_controller, Button.kA.value)
.onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
.onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
}
Expand Down
35 changes: 19 additions & 16 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,33 +9,33 @@

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DutyCycleEncoder;

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

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

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

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() {

}
/**
*
* @param speed motor power to apply to intake
*@param angle in radians
*
*/
public void load(double speed, double angle){
intakeMotor.set(speed);
tiltToAngle(angle);

// Starts intaking the disk
public void intakeDisk() {
intakeMotor.set(IntakeConstants.kIntakeSpeed);
}

//
public void stopIntaking() {
intakeMotor.set(0);
}

/**
Expand All @@ -47,7 +47,10 @@ public void tiltToAngle(double angle) {
double motorPower = armPID.calculate(armEncoder.getAbsolutePosition(), angle);
armMotor.set(motorPower);
}

public void stopRotating(){

}

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

0 comments on commit fd6d287

Please sign in to comment.