Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
  • Loading branch information
MichaelLesirge committed Feb 27, 2024
2 parents e8464cb + 6e79b8c commit 0b47f3e
Show file tree
Hide file tree
Showing 11 changed files with 381 additions and 229 deletions.
74 changes: 38 additions & 36 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
@@ -1,37 +1,39 @@
{
"java.configuration.updateBuildConfiguration": "automatic",
"java.server.launchMode": "Standard",
"files.exclude": {
"**/.git": true,
"**/.svn": true,
"**/.hg": true,
"**/CVS": true,
"**/.DS_Store": true,
"bin/": true,
"**/.classpath": true,
"**/.project": true,
"**/.settings": true,
"**/.factorypath": true,
"**/*~": true
},
"java.test.config": [
{
"name": "WPIlibUnitTests",
"workingDirectory": "${workspaceFolder}/build/jni/release",
"vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
"env": {
"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
"DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
}
},
],
"java.test.defaultConfig": "WPIlibUnitTests",
"cSpell.words": [
"AHRS",
"Deadzone",
"Odometry",
"setpoint",
"velociticities"
]
}

"java.configuration.updateBuildConfiguration": "automatic",
"java.server.launchMode": "Standard",
"files.exclude": {
"**/.git": true,
"**/.svn": true,
"**/.hg": true,
"**/CVS": true,
"**/.DS_Store": true,
"bin/": true,
"**/.classpath": true,
"**/.project": true,
"**/.settings": true,
"**/.factorypath": true,
"**/*~": true
},
"java.test.config": [
{
"name": "WPIlibUnitTests",
"workingDirectory": "${workspaceFolder}/build/jni/release",
"vmargs": [
"-Djava.library.path=${workspaceFolder}/build/jni/release"
],
"env": {
"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
"DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
}
},
],
"java.test.defaultConfig": "WPIlibUnitTests",
// Auto Formatter
"editor.formatOnSave": true, // So you cant forget
"editor.tabSize": 4, // 4 char width indentation
"editor.detectIndentation": false, // Prevent above from being overridden
"editor.insertSpaces": false, // use tab character instead of spaces
"editor.codeActionsOnSave": {
"source.fixAll": "always"
}
}
32 changes: 16 additions & 16 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -66,43 +66,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 @@ -169,7 +169,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
38 changes: 24 additions & 14 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
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.AimAtTag;
import frc.robot.commands.ArmRotateTo;
Expand All @@ -13,6 +12,9 @@
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 com.kauailabs.navx.frc.AHRS;
Expand Down Expand Up @@ -69,12 +71,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 @@ -83,10 +90,9 @@ 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);
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);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
Expand Down Expand Up @@ -134,10 +140,14 @@ public void setUpDriveController() {
joystick.button(1).onTrue(Commands.run(inputs::speedUp));
joystick.button(3).onTrue(Commands.run(inputs::toggleFieldRelative));

//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);
// 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);

joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
} else {
final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());

Expand Down
64 changes: 31 additions & 33 deletions src/main/java/frc/robot/commands/ArmRemoteControl.java
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
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();
}
}
Loading

0 comments on commit 0b47f3e

Please sign in to comment.