Skip to content

Commit

Permalink
Merge branch 'main' into LEDsubSystem
Browse files Browse the repository at this point in the history
  • Loading branch information
RamiRedShift committed Feb 24, 2024
2 parents 0f07631 + 2f6b8eb commit 30f7ca2
Show file tree
Hide file tree
Showing 16 changed files with 437 additions and 135 deletions.
75 changes: 36 additions & 39 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
@@ -1,40 +1,37 @@
{
"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",
// Style guide - most of this came from clicking buttons and then copying from my user settings.json
// General editor stuff
"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
"java.cleanup.actionsOnSave": [
"addOverride",
]
}
"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"
]
}

2 changes: 1 addition & 1 deletion shuffleboard.json
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@
],
"content": {
"_type": "Gyro",
"_source0": "network_table:///SmartDashboard/Heading Degrees",
"_source0": "network_table:///SmartDashboard/ Degrees",
"_title": "Heading Degrees",
"_glyph": 139,
"_showGlyph": true,
Expand Down
51 changes: 37 additions & 14 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,8 @@ public static enum Bot {
public static class DriverConstants {
public static final int DRIVER_JOYSTICK_PORT = 0;

public static final int OPERATOR_JOYSTICK_PORT = 1;

public static final double DEAD_ZONE = 0.25;

// Names of options for displaying
Expand All @@ -65,14 +67,35 @@ public static class DriverConstants {
public static final double[] maxSpeedOptionsRotation = { 0.25, 0.75, 1 };
}

public static class RobotMovementConstants {
public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
public static class ArmConstants {

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 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 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);
public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(5);
public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);

public static final double ROTATION_PID_P = 0.5;
public static final double ROTATION_PID_P = 5;
public static final double ROTATION_PID_I = 0;
public static final double ROTATION_PID_D = 0;

Expand Down Expand Up @@ -121,25 +144,25 @@ public static class SwerveModuleConstants {
VELOCITY_MOTOR_ID_FL = 2;
ANGULAR_MOTOR_ID_FL = 3;
ANGULAR_MOTOR_ENCODER_ID_FL = 3;
ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.256015325670498;
ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.364013671875;

// Front right
VELOCITY_MOTOR_ID_FR = 16;
ANGULAR_MOTOR_ID_FR = 17;
ANGULAR_MOTOR_ENCODER_ID_FR = 2;
ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.248045977011494;
ANGULAR_MOTOR_ENCODER_ID_FR = 4;
ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.42114257812;

// Back left
VELOCITY_MOTOR_ID_BL = 8;
ANGULAR_MOTOR_ID_BL = 9;
ANGULAR_MOTOR_ENCODER_ID_BL = 4;
ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.894674329501916;
ANGULAR_MOTOR_ENCODER_ID_BL = 2;
ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.634033203125;

// Back right
VELOCITY_MOTOR_ID_BR = 10;
ANGULAR_MOTOR_ID_BR = 11;
ANGULAR_MOTOR_ENCODER_ID_BR = 1;
ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.530498084291188;
ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.54736328125;
break;

// case COMPETITION_BOT:
Expand Down Expand Up @@ -170,7 +193,7 @@ public static class SwerveModuleConstants {
public static final double DRIVE_PID_MAX_I = 0.001;

// Steering PID values
public static final double STEERING_PID_P = 0.5;
public static final double STEERING_PID_P = 1;
public static final double STEERING_PID_I = 0;
public static final double STEERING_PID_D = 0;

Expand Down Expand Up @@ -209,8 +232,8 @@ public static class SwerveDrivetrainConstants {

case PRACTICE_BOT:
default: // Temporary default to practice bot
MODULE_LOCATION_X = 54 / 100;
MODULE_LOCATION_Y = 54 / 100;
MODULE_LOCATION_X = 54.0 / 100;
MODULE_LOCATION_Y = 54.0 / 100;
break;

// case COMPETITION_BOT:
Expand Down
45 changes: 35 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,22 @@
import frc.robot.Constants.DriverConstants;
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.commands.Autos;
import frc.robot.commands.ChassisRemoteControl;
import frc.robot.inputs.ChassisDriveInputs;
import frc.robot.inputs.OptionButtonInput;
import frc.robot.inputs.OptionButtonInput.ActivationMode;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.GenericHID.HIDType;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand Down Expand Up @@ -67,21 +69,37 @@ public class RobotContainer {
new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X,
-SwerveDrivetrainConstants.MODULE_LOCATION_Y));

private final AHRS gyro = new AHRS(I2C.Port.kOnboard);
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);


private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
swerveModuleBL, swerveModuleBR);

private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();

private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, Constants.VisionConstants.CAMERA_POSE);
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);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
autoChooser.addOption("Rotate by 90", Autos.rotateBy90Auto(drivetrain));
autoChooser.addOption("Rotate to 90", Autos.rotateTo90Auto(drivetrain));
autoChooser.addOption("Rotate by -90", Autos.rotateByNegative90Auto(drivetrain));
autoChooser.addOption("Rotate to -90", Autos.rotateToNegative90Auto(drivetrain));
autoChooser.addOption("Rotate by 10", Autos.rotateBy10Auto(drivetrain));
SmartDashboard.putData("Auto Chooser", autoChooser);

configureBindings();
Expand All @@ -96,6 +114,8 @@ public void setUpDriveController() {
final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
final HIDType genericHIDType = genericHID.getType();

final CommandJoystick operatorJoystick = new CommandJoystick(DriverConstants.OPERATOR_JOYSTICK_PORT);

SmartDashboard.putString("Drive Controller", genericHIDType.toString());
SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);

Expand All @@ -106,20 +126,24 @@ public void setUpDriveController() {

if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());

inputs = new ChassisDriveInputs(
joystick::getY, -1,
joystick::getX, -1,
joystick::getY, -1,
joystick::getTwist, -1,
Constants.DriverConstants.DEAD_ZONE);

preciseModeButton = new OptionButtonInput(joystick, 2, ActivationMode.TOGGLE);
boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);

joystick.button(10).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
joystick.button(11).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
//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 All @@ -134,7 +158,8 @@ public void setUpDriveController() {
fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
}

drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs, preciseModeButton, boostModeButton, fieldRelativeButton));
drivetrain.setDefaultCommand(
new ChassisRemoteControl(drivetrain, inputs, preciseModeButton, boostModeButton, fieldRelativeButton));
}

/** Use this method to define your trigger->command mappings. */
Expand Down
74 changes: 74 additions & 0 deletions src/main/java/frc/robot/commands/ArmRemoteControl.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
package frc.robot.commands;

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


public class ArmRemoteControl extends Command {
private final Arm 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;

this.raiseArmButton = raiseArmButton;
this.lowerArmButton = lowerArmButton;

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


addRequirements(arm);
}



@Override
public void initialize() {
}

/**
* execute() is called repeatedly while a command is scheduled, about every
* 20ms.
*/
@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();
}

}

@Override
public boolean isFinished() {
return false;
}

@Override
public void end(boolean interrupted) {
}
}
Loading

0 comments on commit 30f7ca2

Please sign in to comment.