Skip to content

Commit

Permalink
Convert Arm class to interface (#3)
Browse files Browse the repository at this point in the history 
* Arm as interface

* Fix type error relating to arm interface
  • Loading branch information
@AceiusRedshift
AceiusRedshift authored Feb 27, 2024
1 parent 250fb5e commit a4d5f81
Show file tree
Hide file tree
Showing 9 changed files with 245 additions and 187 deletions.
32 changes: 16 additions & 16 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,43 +64,43 @@ public static class DriverConstants {
public static final double[] maxSpeedOptionsRotation = { 0.25, 0.75, 1 };
}

public static class ArmConstants {
public static class ArmConstants {
static {
switch (currentBot) {
case WOOD_BOT:
HAR_ARM = false;
HAS_ARM = false;
break;

case COMP_BOT:
default:
HAR_ARM = true;
HAS_ARM = true;
break;
}
}

public static final boolean HAR_ARM;
public static final boolean HAS_ARM;

public static final double MAXIMUM_ARM_DEGREES = 1;
public static final double MINIMUM_ARM_DEGREES = 0;
public static final double MAXIMUM_ARM_DEGREES = 1;
public static final double MINIMUM_ARM_DEGREES = 0;

public static final double ARM_AMP_SHOOTING_DEGREES = -20;
public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45;
public static final double ARM_INTAKE_DEGREES = -40;
public static final double ARM_AMP_SHOOTING_DEGREES = -20;
public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45;
public static final double ARM_INTAKE_DEGREES = -40;

public static final int LEFT_MOTOR_ID = 5;
//public static final int LEFT_ENCODER_ID = 0;
public static final int RIGHT_MOTOR_ID = 19;
public static final int RIGHT_ENCODER_ID = 6;
public static final int LEFT_MOTOR_ID = 5;
// public static final int LEFT_ENCODER_ID = 0;
public static final int RIGHT_MOTOR_ID = 19;
public static final int RIGHT_ENCODER_ID = 6;

public static final boolean ARE_MOTORS_REVERSED = false;

public static final double DEGREES_PER_SECOND = 2.0;
public static final double DEGREES_PER_SECOND = 2.0;

public static final double ELEVATION_PID_P = 15;
public static final double ELEVATION_PID_I = 0;
public static final double ELEVATION_PID_D = 0;

}
}

public static class RobotMovementConstants {
public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
Expand Down Expand Up @@ -167,7 +167,7 @@ public static class SwerveModuleConstants {
VELOCITY_MOTOR_ID_BL = 8;
ANGULAR_MOTOR_ID_BL = 9;
ANGULAR_MOTOR_ENCODER_ID_BL = 2;
ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 +0.5;
ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 + 0.5;

// Back right
VELOCITY_MOTOR_ID_BR = 10;
Expand Down
23 changes: 15 additions & 8 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,16 @@
import frc.robot.Constants.SwerveDrivetrainConstants;
import frc.robot.Constants.SwerveModuleConstants;
import frc.robot.Constants.ArmConstants;
import frc.robot.subsystems.Arm;
import frc.robot.commands.Autos;
import frc.robot.commands.ArmRotateTo;
import frc.robot.commands.ChassisRemoteControl;
import frc.robot.Constants.VisionConstants;
import frc.robot.subsystems.SwerveDrivetrain;
import frc.robot.subsystems.SwerveModule;
import frc.robot.subsystems.Vision;
import frc.robot.subsystems.arm.ArmInterface;
import frc.robot.subsystems.arm.DummyArm;
import frc.robot.subsystems.arm.RealArm;
import frc.robot.inputs.ChassisDriveInputs;
import frc.robot.inputs.OptionButtonInput;
import frc.robot.inputs.OptionButtonInput.ActivationMode;
Expand Down Expand Up @@ -70,12 +72,17 @@ public class RobotContainer {
-SwerveDrivetrainConstants.MODULE_LOCATION_Y));

private final AHRS gyro = new AHRS();
private final Arm arm = new Arm(
ArmConstants.LEFT_MOTOR_ID,
ArmConstants.RIGHT_MOTOR_ID,
ArmConstants.RIGHT_ENCODER_ID,
ArmConstants.ARE_MOTORS_REVERSED);

/**
* This is the robot arm. In some situations, the robot may not have an arm, so
* if ArmConstants.HAS_ARM is false, a dummy class implementing the arm's API is
* created instead to prevent errors.
*/
private final ArmInterface arm = Constants.ArmConstants.HAS_ARM ? new RealArm(
ArmConstants.LEFT_MOTOR_ID,
ArmConstants.RIGHT_MOTOR_ID,
ArmConstants.RIGHT_ENCODER_ID,
ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm();

private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
swerveModuleBL, swerveModuleBR);
Expand All @@ -84,7 +91,6 @@ public class RobotContainer {

private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE);


private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
Expand Down Expand Up @@ -137,7 +143,8 @@ public void setUpDriveController() {
boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);

//This bypasses arm remote control, arm remote control is incompatible with autonomous commands
// This bypasses arm remote control, arm remote control is incompatible with
// autonomous commands
operatorJoystick.button(4).onTrue(armToIntake);
operatorJoystick.button(5).onTrue(armToAmp);
operatorJoystick.button(6).onTrue(armToSpeaker);
Expand Down
64 changes: 31 additions & 33 deletions src/main/java/frc/robot/commands/ArmRemoteControl.java
View file
Original file line number Diff line number Diff line change
@@ -1,40 +1,36 @@
package frc.robot.commands;

import frc.robot.Constants.ArmConstants;
import frc.robot.subsystems.Arm;
import frc.robot.inputs.OptionButtonInput;
import frc.robot.subsystems.arm.ArmInterface;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;


public class ArmRemoteControl extends Command {
private final Arm arm;
private final ArmInterface arm;

private final OptionButtonInput raiseArmButton;
private final OptionButtonInput lowerArmButton;

private final OptionButtonInput speakerPositionButton;
private final OptionButtonInput ampPositionButton;
private final OptionButtonInput intakePositionButton;


public ArmRemoteControl(Arm arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton, OptionButtonInput intakePositionButton) {
this.arm = arm;
private final OptionButtonInput speakerPositionButton;
private final OptionButtonInput ampPositionButton;
private final OptionButtonInput intakePositionButton;

this.raiseArmButton = raiseArmButton;
this.lowerArmButton = lowerArmButton;
public ArmRemoteControl(ArmInterface arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton,
OptionButtonInput intakePositionButton) {
this.arm = arm;

this.speakerPositionButton = speakerPositionButton;
this.ampPositionButton = ampPositionButton;
this.intakePositionButton = intakePositionButton;
this.raiseArmButton = raiseArmButton;
this.lowerArmButton = lowerArmButton;

this.speakerPositionButton = speakerPositionButton;
this.ampPositionButton = ampPositionButton;
this.intakePositionButton = intakePositionButton;

addRequirements(arm);
}



@Override
public void initialize() {
}
Expand All @@ -46,24 +42,26 @@ public void initialize() {
@Override
public void execute() {

// Arm Motor
arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);

// Arm Povs
if (speakerPositionButton.getState()) {
arm.setArmToSpeakerPosition();
}
if (ampPositionButton.getState()) {
arm.setArmToAmpPosition();
}
if (intakePositionButton.getState()) {
arm.setArmToIntakePosition();
}
// Arm Motor
arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
* ArmConstants.DEGREES_PER_SECOND);
arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
* ArmConstants.DEGREES_PER_SECOND);

// Arm Povs
if (speakerPositionButton.getState()) {
arm.setArmToSpeakerPosition();
}
if (ampPositionButton.getState()) {
arm.setArmToAmpPosition();
}
if (intakePositionButton.getState()) {
arm.setArmToIntakePosition();
}

}

@Override
@Override
public boolean isFinished() {
return false;
}
Expand Down
34 changes: 17 additions & 17 deletions src/main/java/frc/robot/commands/ArmRotateTo.java
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,28 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.arm.ArmInterface;

public class ArmRotateTo extends Command{
public class ArmRotateTo extends Command {

private final double setpoint;
private final Arm arm;
private final double setpoint;
private final ArmInterface arm;

public ArmRotateTo(Arm arm, double degree){
this.setpoint = degree;
this.arm = arm;
public ArmRotateTo(ArmInterface arm, double degree) {
this.setpoint = degree;
this.arm = arm;

addRequirements(arm);
addRequirements(arm);

}
}

@Override
public void initialize() {
arm.setSetpoint(setpoint);
}
@Override
public void initialize() {
arm.setSetpoint(setpoint);
}

@Override
public boolean isFinished() {
return arm.isAtDesiredPosition();
}
@Override
public boolean isFinished() {
return arm.isAtDesiredPosition();
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/Autos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,12 @@

import frc.robot.subsystems.SwerveDrivetrain;
import frc.robot.subsystems.Vision;
import frc.robot.subsystems.arm.ArmInterface;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.SwerveDrivetrainConstants;
import frc.robot.subsystems.Arm;

/**
* This class just contains a bunch of auto-modes. Do not call this class
Expand Down Expand Up @@ -50,7 +50,7 @@ public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera,
}

/** Linden did this */
public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
public static Command startingAuto(ArmInterface arm, SwerveDrivetrain drivetrain, boolean invertY) {

// assumes start position in corner
double invert = 1;
Expand Down
Loading

0 comments on commit a4d5f81

Please sign in to comment.