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
@BrandonS09
BrandonS09 committed Mar 3, 2024
2 parents a9d3dfa + 92e51b8 commit 48c0c01
Show file tree
Hide file tree
Showing 3 changed files with 53 additions and 29 deletions.
67 changes: 48 additions & 19 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,10 +25,26 @@ public static final class Drivetrain {
}

public static final class Arm {

//ports
//config for motors (inverted, encoder offset, etc)
//goal positions
//feedforward, pid, trapezoid constants
//Boundaries (arm cannot go beyond certain angles)
//Controller buttons should go in OI not Arm

//Motor port
public static final int ARM_MOTOR_PORT_1 = 7;
public final static int ARM_MOTOR_PORT_2 = 8;
//all angles in rot here
//Config for motors
public static final boolean MOTOR_INVERTED = true; //Todo: find all these (they are definetely wrong)
public static final double ROTATION_TO_RAD = 2 * Math.PI;
public static final boolean ENCODER_INVERTED = false;
public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0;
public static final double POS_TOLERANCE_RAD = 0;
public static final double VEL_TOLERANCE_RAD_P_SEC = 0;
public static final int MAX_VOLTAGE = 12;

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

// USE RADIANS FOR THE ARM
Expand All @@ -40,31 +57,22 @@ public static final class Arm {
public static final double climberDownAngle = Math.toRadians(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
//PID, Feedforward, Trapezoid
public static final double kP = 0.1;
public static final double kI = 0.1;
public static final double kD = 0.1;
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;

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

//if needed
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 TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);

//Arm buttons
Expand All @@ -88,6 +96,20 @@ public static final class Arm {
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 class IntakeShooter {
Expand All @@ -105,6 +127,13 @@ public static final class Driver {
}
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_CLIMBER_BUTTON = Button.kLeftBumper.value;
public static final int LOWER_TO_CLIMBER_BUTTON = Button.kRightBumper.value;
}
}
}
4 changes: 2 additions & 2 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,8 @@ private void setBindingsManipulator() {
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.Arm.RAISE_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberUpAngle);}));
new JoystickButton(manipulatorController, Constants.Arm.LOWER_TO_CLIMBER_BUTTON).onTrue(new InstantCommand(() -> {arm.driveArm(Constants.Arm.climberDownAngle);}));
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
11 changes: 3 additions & 8 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ public void periodic() {

// SmartDashboard.putNumber("MaxHoldingTorque", maxHoldingTorqueNM());
//SmartDashboard.putNumber("V_PER_NM", getV_PER_NM());
SmartDashboard.putNumber("COMDistance", getCoM().getNorm());
// SmartDashboard.putNumber("COMDistance", getCoM().getNorm());
SmartDashboard.putNumber("InternalArmVelocity", armEncoder.getVelocity());
//SmartDashboard.putNumber("Arm Current", armMotor.getOutputCurrent());

Expand Down Expand Up @@ -197,13 +197,8 @@ public double getArmClampedGoal(double goal) {



public Translation2d getCoM() {
Translation2d comOfArm = new Translation2d(COM_ARM_LENGTH_METERS, Rotation2d.fromRadians(getArmPos()))
.times(ARM_MASS_KG);

return comOfArm.plus(comOfArm);
//this math is prob wront
}




}

0 comments on commit 48c0c01

Please sign in to comment.