Tried implementing trapezoidal profile

DeepBlueRobotics · Feb 3, 2024 · b2b20e2 · b2b20e2
1 parent 47e0138
commit b2b20e2
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 3 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -51,7 +51,6 @@ public static final class Arm {
 		//Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING
 		public static final double COM_ARM_LENGTH_METERS = 1;
 		public static final double ARM_MASS_KG = 1;
-
 		public static final double MAX_FF_VEL = 1; // rot / s
 		public static final double MAX_FF_ACCEL = 1; // rot / s^2
 		public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -46,7 +46,7 @@ public RobotContainer() {
 
   private void setDefaultCommands() {
 
-    //arm.setDefaultCommand(new InstantCommand(() -> {arm.driveArm(() -> ProcessedAxisValue(driverController, Axis.kLeftY));}));
+    arm.setDefaultCommand(new InstantCommand(() -> {arm.driveArm());
 
     // drivetrain.setDefaultCommand(new TeleopDrive(
     //   drivetrain,

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -40,6 +40,8 @@ public class Arm extends SubsystemBase {
     private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
     private final SparkAbsoluteEncoder armEncoder = armMotor1.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
     private final PIDController armPID = new PIDController(Constants.Arm.kP, Constants.Arm.kI, Constants.Arm.kD);
+    public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(Math.toRadians(43), 0) };
+    private TrapezoidProfile armProfile = new TrapezoidProfile(Constants.Arm.trapConstraints, goalState[0], getCurrentArmState());
     public Arm() {
 			//arm 
       /*
@@ -77,6 +79,10 @@ public Translation2d getCoM() {
       return comOfArm;
     }
 
+    public double getArmVel() {
+      return armEncoder.getVelocity();
+  }
+
     public void driveArm(TrapezoidProfile.State state) {
 
       /*
@@ -98,8 +104,12 @@ public double getArmClampedGoal(double goal) {
       return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE_LIMIT, Constants.Arm.UPPER_ANGLE_LIMIT);
     }
 
+    public TrapezoidProfile.State getCurrentArmState() {
+      return new TrapezoidProfile.State(getArmPos(), getArmVel());
+    }
+
     @Override
     public void periodic() {
-      //run driveArm constantly and pass in 
+
 		}
 }