Skip to content

Commit

Permalink
formatted constants
Browse files Browse the repository at this point in the history
  • Loading branch information
@stwiggy
stwiggy committed Mar 3, 2024
1 parent e0b5447 commit f4b812c
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 58 deletions.
83 changes: 43 additions & 40 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units.*;
Expand All @@ -24,72 +25,65 @@ public static final class Drivetrain {
}

public static final class Arm {
//Motor port
//ports
public static final int ARM_MOTOR_PORT_1 = 7;
public final static int ARM_MOTOR_PORT_2 = 8;
//all angles in rot here
//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 intakeAngle = Math.toRadians(0);
public static final double ampAngle = Math.toRadians(103);
public static final double placeholderSpeakerAngle1 = Math.toRadians(24);
public static final double placeholderSpeakerAngle2 = Math.toRadians(24);
public static final double placeholderSpeakerAngle3 = Math.toRadians(24);
public static final double climberUpAngle = Math.toRadians(24);
public static final double climberDownAngle = Math.toRadians(24);
//config
public static final boolean MOTOR_INVERTED = true; //Todo: find all these (they are definetely wrong)
public static final boolean ENCODER_INVERTED = false;

//goal pos
// 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); //placeholder
public static final double SPEAKER_ANGLE_2 = Units.degreesToRadians(24); //placeholder
public static final double SPEAKER_ANGLE_3 = Units.degreesToRadians(24); //placeholder
public static final double CLIMBER_UP_ANGLE = Units.degreesToRadians(24);
public static final double CLIMBER_DOWN_ANGLE = Units.degreesToRadians(24);

//if needed
public static final double UPPER_ANGLE_LIMIT = Math.toRadians(70);
public static final double LOWER_ANGLE_LIMIT = Math.toRadians(0);
public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
// Feedforward
public static final double kS = 0.1;
public static final double kG = 0.1;
public static final double kV = 0.1;
public static final double kA = 0.1;
public static final double IZONE = 4;

// PID Constants
// placeholder numbers for now
public static final double kP = 0.1;
public static final double kI = 0.1;
public static final double kD = 0.1;
//I assume to max vel and accel are in meters per second

public static final int MAX_VOLTAGE = 12;


//Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING
public static final double COM_ARM_LENGTH_METERS = 0.381 ;
public static final double ARM_MASS_KG = 9.59302503;
//trapezoidal
public static final double MAX_FF_VEL = 1; // rot / s
public static final double MAX_FF_ACCEL = 1; // rot / s^2
//I assume to max vel and accel are in meters per second
public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);
public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);
public static final int MAX_VOLTAGE = 12;
public static final double ROTATION_TO_RAD = 2 * Math.PI;

//Arm buttons
public static final int raiseToSpeakerPodButton = Button.kY.value;
public static final int raiseToAmpButton = Button.kB.value;
public static final int raiseToSpeakerSafeButton = Button.kA.value;
public static final int raiseToSpeakerNextButton = Button.kX.value;
public static final int raiseToGroundButton = Button.kStart.value;
public static final int raiseToClimberButton = Button.kLeftBumper.value;
public static final int lowerToClimberButton = Button.kRightBumper.value;
//other
public static final boolean motorInverted = true; //Todo: find all these (they are definetely wrong)
public static final double rotationToRad = 2 * Math.PI;
public static final boolean encoderInverted = false;
public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0;
public static final double posToleranceRad = 0;
public static final double velToleranceRadPSec = 0;

//boundaries
public static final double UPPER_ANGLE_LIMIT = Units.degreesToRadians(70);
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 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_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0;
public static final double ARM_DISCONTINUITY_RAD = (ARM_LOWER_LIMIT_RAD + ARM_UPPER_LIMIT_RAD) / 2 - Math.PI;
public static final double POS_TOLERANCE_RAD = 0;
public static final double VEL_TOLERANCE_RAD_PER_SEC = 0;

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

//Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING
public static final double COM_ARM_LENGTH_METERS = 0.381 ;
public static final double ARM_MASS_KG = 9.59302503;

}

public static final class IntakeShooter {
//in set() speed
public static final double idleSpeed = 0;
Expand All @@ -105,6 +99,15 @@ public static final class Driver {
}
public static final class Manipulator {
public static final int port = 1;

//Arm buttons
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_CLIMBER_BUTTON = Button.kLeftBumper.value;
public static final int LOWER_TO_CLIMER_BUTTON = Button.kRightBumper.value;
}
}
}
15 changes: 7 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,6 @@
// import org.carlmontrobotics.subsystems.*;
import org.carlmontrobotics.commands.*;
import static org.carlmontrobotics.Constants.OI;
import static org.carlmontrobotics.Constants.Arm.ampAngle;

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

// Speaker Buttons
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerPodButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle1);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerNextButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle2);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerSafeButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.placeholderSpeakerAngle3);}));
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);}));
// Amp and Intake Buttons
new JoystickButton(manipulatorController, Constants.Arm.raiseToAmpButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.ampAngle);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToGroundButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.intakeAngle);}));
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);}));
// Cimber Buttons
new JoystickButton(manipulatorController, Constants.Arm.raiseToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberUpAngle);}));
new JoystickButton(manipulatorController, Constants.Arm.lowerToClimberButton).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberDownAngle);}));
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_CLIMER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.CLIMBER_DOWN_ANGLE);}));
}

public Command getAutonomousCommand() {
Expand Down
25 changes: 15 additions & 10 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ public class Arm extends SubsystemBase {
private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(kS, kV, kA);
private final SparkPIDController armPID1 = armMotor1.getPIDController();
private final SparkPIDController armPID2 = armMotor2.getPIDController();
private TrapezoidProfile armProfile = new TrapezoidProfile(trapConstraints);
private TrapezoidProfile armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
private Timer armProfileTimer = new Timer();
TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later

Expand All @@ -60,13 +60,13 @@ public class Arm extends SubsystemBase {

public Arm() {
// weird math stuff
armMotor1.setInverted(motorInverted);
armMotor1.setInverted(MOTOR_INVERTED);
armMotor1.setIdleMode(IdleMode.kBrake);
armMotor2.setInverted(motorInverted);
armMotor2.setInverted(MOTOR_INVERTED);
armMotor2.setIdleMode(IdleMode.kBrake);
armEncoder.setPositionConversionFactor(rotationToRad);
armEncoder.setVelocityConversionFactor(rotationToRad);
armEncoder.setInverted(encoderInverted);
armEncoder.setPositionConversionFactor(ROTATION_TO_RAD);
armEncoder.setVelocityConversionFactor(ROTATION_TO_RAD);
armEncoder.setInverted(ENCODER_INVERTED);

armMotor2.follow(armMotor1);

Expand All @@ -82,8 +82,8 @@ public Arm() {

// SmartDashboard.putNumber("Arm Max Vel", MAX_FF_VEL );
SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad);
SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec);
SmartDashboard.putNumber("Arm Tolerance Pos", POS_TOLERANCE_RAD);
SmartDashboard.putNumber("Arm Tolerance Vel", VEL_TOLERANCE_RAD_PER_SEC);
}

@Override
Expand Down Expand Up @@ -119,6 +119,11 @@ public void periodic() {
autoCancelArmCommand();
}

public TrapezoidProfile.State calculateCustomSetPoint(double goalSeconds, TrapezoidProfile.State currentPoint, TrapezoidProfile.State goalState) {
return armProfile.calculate(goalSeconds, currentPoint, goalState);

}

public void autoCancelArmCommand() {
if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return;

Expand All @@ -144,7 +149,7 @@ public void driveArm(double goalAngle){
public void setArmTarget(double targetPos) {
targetPos = getArmClampedGoal(targetPos);

armProfile = new TrapezoidProfile(trapConstraints);
armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
armProfileTimer.reset();

goalState.position = targetPos;
Expand All @@ -156,7 +161,7 @@ public void setArmTarget(double targetPos) {
public void resetGoal() {
double armPos = getArmPos();

armProfile = new TrapezoidProfile(trapConstraints);
armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);

}

Expand Down

0 comments on commit f4b812c

Please sign in to comment.