Skip to content

Commit

Permalink
ran alt shift f for everything
Browse files Browse the repository at this point in the history
  • Loading branch information
@BrandonS09
BrandonS09 committed Mar 5, 2024
1 parent dd1ac71 commit ca60b94
Show file tree
Hide file tree
Showing 6 changed files with 209 additions and 149 deletions.
92 changes: 48 additions & 44 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,33 +13,37 @@
import edu.wpi.first.wpilibj.XboxController.Button;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean
* constants. This class should not be used for any other purpose. All constants
* should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class Drivetrain {
}

public static final class Arm {

//Motor port
// Motor port
public static final int ARM_MOTOR_PORT_MASTER = 13;
public final static int ARM_MOTOR_PORT_FOLLOWER = 18;
//Config for motors
public static final boolean MOTOR_INVERTED_MASTER = true; //Todo: find all these (they are definetely wrong)
// Config for motors
public static final boolean MOTOR_INVERTED_MASTER = true; // Todo: find all these (they are definetely wrong)
public static final boolean MOTOR_INVERTED_FOLLOWER = false;
public static final double ROTATION_TO_RAD = 2 * Math.PI;
public static final boolean ENCODER_INVERTED = false;



public static final int MAX_VOLTAGE = 12;
public static final double ENCODER_OFFSET_RAD = 0;

//TODO: finish understand why this is broken public static final Measure<Angle> INTAKE_ANGLE = Degrees.to(-1);

// TODO: finish understand why this is broken public static final Measure<Angle>
// INTAKE_ANGLE = Degrees.to(-1);

// USE RADIANS FOR THE ARM
public static final double INTAKE_ANGLE_RAD = Units.degreesToRadians(0);
Expand All @@ -50,8 +54,7 @@ public static final class Arm {
public static final double CLIMBER_UP_ANGLE_RAD = Units.degreesToRadians(24);
public static final double CLIMBER_DOWN_ANGLE_RAD = Units.degreesToRadians(24);


//PID, Feedforward, Trapezoid
// PID, Feedforward, Trapezoid
public static final double kP = 0.1;
public static final double kI = 0.1;
public static final double kD = 0.1;
Expand All @@ -61,53 +64,54 @@ public static final class Arm {
public static final double kA = 0.1;
public static final double IZONE = 4;
public static final double MAX_FF_VEL_RAD_P_S = 1; // rad / s
public static final double MAX_FF_ACCEL_RAD_P_S = 1; // rad / s^2
public static final double MAX_FF_ACCEL_RAD_P_S = 1; // rad / s^2

//if needed
public static final double COM_ARM_LENGTH_METERS = 0.381 ;
// if needed
public static final double COM_ARM_LENGTH_METERS = 0.381;
public static final double ARM_MASS_KG = 9.59302503;

public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_FF_VEL_RAD_P_S, MAX_FF_ACCEL_RAD_P_S);
//other0;

public static final double MARGIN_OF_ERROR = Math.PI/18;
//Boundaries
public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; //placeholder
public static final double POS_TOLERANCE_RAD = 0; //placeholder
public static final double VEL_TOLERANCE_RAD_P_SEC = 0; //placeholder
public static final double UPPER_ANGLE_LIMIT_RAD = Units.degreesToRadians(70);

public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(
MAX_FF_VEL_RAD_P_S, MAX_FF_ACCEL_RAD_P_S);
// other0;

public static final double MARGIN_OF_ERROR = Math.PI / 18;

// Boundaries
public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; // placeholder
public static final double POS_TOLERANCE_RAD = 0; // placeholder
public static final double VEL_TOLERANCE_RAD_P_SEC = 0; // placeholder
public static final double UPPER_ANGLE_LIMIT_RAD = Units.degreesToRadians(70);
public static final double LOWER_ANGLE_LIMIT_RAD = Units.degreesToRadians(0);
public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) /2 - Math.PI;


//Arm buttons

public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI;

// Arm buttons

}

public static final class IntakeShooter {
//in set() speed
// in set() speed
public static final double idleSpeed = 0;
public static final double intakeSpeed = .7;
public static final double outtakeSpeed = 1;

}

public static final class OI {
public static final double JOY_THRESH = 0.01;
public static final class Driver {
public static final int port = 0;
}
public static final class Manipulator {
public static final int port = 1;

public static final class Driver {
public static final int port = 0;
}

public static final class Manipulator {
public static final int port = 1;
public static final int RAISE_TO_SPEAKER_POD_BUTTON = Button.kY.value;
public static final int RAISE_TO_AMP_BUTTON = Button.kB.value;
public static final int RAISE_TO_SPEAKER_SAFE_BUTTON = Button.kA.value;
public static final int RAISE_TO_SPEAKER_NEXT_BUTTON = Button.kX.value;
public static final int RAISE_TO_GROUND_BUTTON= Button.kStart.value;
public static final int RAISE_TO_GROUND_BUTTON = Button.kStart.value;
public static final int RAISE_TO_CLIMBER_BUTTON = Button.kLeftBumper.value;
public static final int LOWER_TO_CLIMBER_BUTTON = Button.kRightBumper.value;
}
}
}
}
}
3 changes: 2 additions & 1 deletion src/main/java/org/carlmontrobotics/Main.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@
import edu.wpi.first.wpilibj.RobotBase;

public final class Main {
private Main() {}
private Main() {
}

public static void main(String... args) {
RobotBase.startRobot(Robot::new);
Expand Down
27 changes: 18 additions & 9 deletions src/main/java/org/carlmontrobotics/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,16 @@ public void robotPeriodic() {
}

@Override
public void disabledInit() {}
public void disabledInit() {
}

@Override
public void disabledPeriodic() {}
public void disabledPeriodic() {
}

@Override
public void disabledExit() {}
public void disabledExit() {
}

@Override
public void autonomousInit() {
Expand All @@ -42,10 +45,12 @@ public void autonomousInit() {
}

@Override
public void autonomousPeriodic() {}
public void autonomousPeriodic() {
}

@Override
public void autonomousExit() {}
public void autonomousExit() {
}

@Override
public void teleopInit() {
Expand All @@ -55,19 +60,23 @@ public void teleopInit() {
}

@Override
public void teleopPeriodic() {}
public void teleopPeriodic() {
}

@Override
public void teleopExit() {}
public void teleopExit() {
}

@Override
public void testInit() {
CommandScheduler.getInstance().cancelAll();
}

@Override
public void testPeriodic() {}
public void testPeriodic() {
}

@Override
public void testExit() {}
public void testExit() {
}
}
97 changes: 64 additions & 33 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics;

//199 files
// import org.carlmontrobotics.subsystems.*;
import org.carlmontrobotics.commands.*;
Expand Down Expand Up @@ -30,16 +31,16 @@
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;

public class RobotContainer {
//defaultCommands: elevator, dt
//(pass in controller!)
// defaultCommands: elevator, dt
// (pass in controller!)

//set up subsystems / controllers / limelight
//1. using GenericHID allows us to use different kinds of controllers
//2. Use absolute paths from constants to reduce confusion
// set up subsystems / controllers / limelight
// 1. using GenericHID allows us to use different kinds of controllers
// 2. Use absolute paths from constants to reduce confusion
public final GenericHID driverController = new GenericHID(OI.Driver.port);
public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port);

public Arm arm = new Arm();
public Arm arm = new Arm();

public RobotContainer() {

Expand All @@ -51,59 +52,87 @@ public RobotContainer() {
private void setDefaultCommands() {

// drivetrain.setDefaultCommand(new TeleopDrive(
// drivetrain,
// () -> ProcessedAxisValue(driverController, Axis.kLeftY)),
// () -> ProcessedAxisValue(driverController, Axis.kLeftX)),
// () -> ProcessedAxisValue(driverController, Axis.kRightX)),
// () -> driverController.getRawButton(OI.Driver.slowDriveButton)
// drivetrain,
// () -> ProcessedAxisValue(driverController, Axis.kLeftY)),
// () -> ProcessedAxisValue(driverController, Axis.kLeftX)),
// () -> ProcessedAxisValue(driverController, Axis.kRightX)),
// () -> driverController.getRawButton(OI.Driver.slowDriveButton)
// ));

arm.setDefaultCommand(new ArmTeleop(arm, () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY))));
}

private void setBindingsDriver() {
new JoystickButton(driverController, Button.kX.value).whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
new JoystickButton(driverController, Button.kY.value).whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
new JoystickButton(driverController, Button.kX.value)
.whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
new JoystickButton(driverController, Button.kY.value)
.whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
new JoystickButton(driverController, Button.kB.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
new JoystickButton(driverController, Button.kA.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
// 4 cardinal directions on arrowpad
// slowmode toggle on trigger
// 3 cardinal directions on letterpad
// slowmode toggle on trigger
// 3 cardinal directions on letterpad
}

private void setBindingsManipulator() {
// intake/outtake on triggers
//Left trigger will be intake whilst right trigger will be outtake
//3 setpositions of arm on letterpad
//Up is Speaker, down is ground, right is Amp
//right joystick used for manual arm control
// intake/outtake on triggers
// Left trigger will be intake whilst right trigger will be outtake
// 3 setpositions of arm on letterpad
// Up is Speaker, down is ground, right is Amp
// right joystick used for manual arm control

// Speaker Buttons
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_POD_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(PODIUM_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_NEXT_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(SUBWOFFER_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_SAFE_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(SAFE_ZONE_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_POD_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(PODIUM_ANGLE_RAD);
}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_NEXT_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(SUBWOFFER_ANGLE_RAD);
}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_SAFE_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(SAFE_ZONE_ANGLE_RAD);
}));
// Amp and Intake Buttons
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_AMP_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(AMP_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_GROUND_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(INTAKE_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_AMP_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(AMP_ANGLE_RAD);
}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_GROUND_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(INTAKE_ANGLE_RAD);
}));
// Cimber Buttons
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(CLIMBER_UP_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.LOWER_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.setArmTarget(CLIMBER_DOWN_ANGLE_RAD);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_CLIMBER_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(CLIMBER_UP_ANGLE_RAD);
}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.LOWER_TO_CLIMBER_BUTTON)
.onTrue(new InstantCommand(() -> {
arm.setArmTarget(CLIMBER_DOWN_ANGLE_RAD);
}));
}

public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
}

/**
* Flips an axis' Y coordinates upside down, but only if the select axis is a joystick axis
* Flips an axis' Y coordinates upside down, but only if the select axis is a
* joystick axis
*
* @param hid The controller/plane joystick the axis is on
* @param hid The controller/plane joystick the axis is on
* @param axis The processed axis
* @return The processed value.
*/
private double getStickValue(GenericHID hid, Axis axis) {
return hid.getRawAxis(axis.value) * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1);
}

/**
* Processes an input from the joystick into a value between -1 and 1, sinusoidally instead of linearly
* Processes an input from the joystick into a value between -1 and 1,
* sinusoidally instead of linearly
*
* @param value The value to be processed.
* @return The processed value.
Expand All @@ -116,14 +145,16 @@ private double inputProcessing(double value) {
value);
return processedInput;
}

/**
* Combines both getStickValue and inputProcessing into a single function for processing joystick outputs
* Combines both getStickValue and inputProcessing into a single function for
* processing joystick outputs
*
* @param hid The controller/plane joystick the axis is on
* @param hid The controller/plane joystick the axis is on
* @param axis The processed axis
* @return The processed value.
*/
private double ProcessedAxisValue(GenericHID hid, Axis axis){
private double ProcessedAxisValue(GenericHID hid, Axis axis) {
return inputProcessing(getStickValue(hid, axis));
}
}
Loading

0 comments on commit ca60b94

Please sign in to comment.