Skip to content

Commit

Permalink
Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/Robot…
Browse files Browse the repository at this point in the history 
…Code2024 into sofie-arm
  • Loading branch information
@sofiebudman
sofiebudman committed Mar 3, 2024
2 parents aa97468 + d2608ba commit 7814fcd
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 29 deletions.
36 changes: 15 additions & 21 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,15 +47,16 @@ public static final class Arm {

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

// Goal Positions
// USE RADIANS FOR THE ARM
public static final double INTAKE_ANGLE = Units.degreesToRadians(0);
public static final double AMP_ANGLE = Units.degreesToRadians(103);
public static final double SPEAKER_ANGLE_1 =Units.degreesToRadians(24);
public static final double SPEAKER_ANGLE_2 = Units.degreesToRadians(24);
public static final double SPEAKER_ANGLE_3 = Units.degreesToRotations(24);
public static final double AMP_ANGLE = Units.degreesToRadians(105);
public static final double SUBWOFFER_ANGLE = Units.degreesToRadians(24);
public static final double SAFE_ZONE_ANGLE = Units.degreesToRadians(24);
public static final double PODIUM_ANGLE = Units.degreesToRadians(24);
public static final double CLIMBER_UP_ANGLE = Units.degreesToRadians(24);
public static final double CLIMBER_DOWN_ANGLE = Units.degreesToRadians(24);


//PID, Feedforward, Trapezoid
public static final double kP = 0.1;
public static final double kI = 0.1;
Expand All @@ -73,28 +74,21 @@ public static final class Arm {
public static final double LOWER_ANGLE_LIMIT = Units.degreesToRadians(0);
public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);

//boundaries
//other0;

public static final double MARGIN_OF_ERROR = Math.PI/18;
public static final double ARM_LOWER_LIMIT_RAD = -3.569 + MARGIN_OF_ERROR;
public static final double ARM_UPPER_LIMIT_RAD = .36 - MARGIN_OF_ERROR;

public static final double ARM_DISCONTINUITY_RAD = (ARM_LOWER_LIMIT_RAD + ARM_UPPER_LIMIT_RAD) / 2 - Math.PI;








//Arm buttons

//other




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_ZONE_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_CLIMBER_BUTTON = Button.kLeftBumper.value;
public static final int LOWER_TO_CLIMBER_BUTTON = Button.kRightBumper.value;
}
public static final class IntakeShooter {
//in set() speed
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
// import org.carlmontrobotics.subsystems.*;
import org.carlmontrobotics.commands.*;
import static org.carlmontrobotics.Constants.OI;

import static org.carlmontrobotics.Constants.Arm.*;

import org.carlmontrobotics.Constants.OI;
import org.carlmontrobotics.subsystems.Arm;
Expand Down Expand Up @@ -72,15 +72,15 @@ private void setBindingsManipulator() {
//right joystick used for manual arm control

// Speaker Buttons
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_POD_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SPEAKER_ANGLE_1);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_NEXT_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SPEAKER_ANGLE_2);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_SPEAKER_SAFE_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SPEAKER_ANGLE_3);}));
new JoystickButton(manipulatorController, Constants.Arm.RAISE_TO_SPEAKER_POD_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.PODIUM_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.RAISE_TO_SPEAKER_NEXT_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SUBWOFFER_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.RAISE_TO_SPEAKER_SAFE_ZONE_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.SAFE_ZONE_ANGLE);}));
// Amp and Intake Buttons
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_AMP_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.AMP_ANGLE);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_GROUND_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.INTAKE_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.RAISE_TO_AMP_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.AMP_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.RAISE_TO_GROUND_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.INTAKE_ANGLE);}));
// Cimber Buttons
new JoystickButton(manipulatorController, Constants.OI.Manipulator.RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_UP_ANGLE);}));
new JoystickButton(manipulatorController, Constants.OI.Manipulator.LOWER_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_DOWN_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_UP_ANGLE);}));
new JoystickButton(manipulatorController, Constants.Arm.LOWER_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_DOWN_ANGLE);}));
}

public Command getAutonomousCommand() {
Expand Down

0 comments on commit 7814fcd

Please sign in to comment.