Kenneth merge the thing

DeepBlueRobotics · Feb 20, 2024 · 247d9d7 · 247d9d7
2 parents 28ca8a5 + a28d22c
commit 247d9d7
Showing 1 changed file with 12 additions and 2 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -27,6 +27,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -94,7 +95,7 @@ public void COMBINE_PID_FF_TRAPEZOID(TrapezoidProfile.State setPoint) {
     public double getArmPos() {
       return MathUtil.inputModulus(armEncoder.getPosition(), Constants.Arm.ARM_DICONT_RAD,
               Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI);
-  }
+    } 
 
 
     public void cancelArmCommand() {
@@ -111,9 +112,10 @@ public Translation2d getCoM() {
 
     public double getArmVel() {
       return armEncoder.getVelocity();
-  }
+    }
 
     public void driveArm(TrapezoidProfile.State state) {
+
 
       /*
       ignore this math its wrong as it includes wrist 
@@ -126,8 +128,16 @@ public void driveArm(TrapezoidProfile.State state) {
     }
     double volts = armFeedVolts + armPIDVolts;
     armMotor1.setVoltage(volts);
+      
   
   */
+      double armFeedVolts = 0; //<-- similar math to above to get this
+      double armPIDVolts = 0;//<--similar math to 
+      SmartDashboard.putNumber("ArmFeedVolts", armFeedVolts);
+      SmartDashboard.putNumber("ArmPIDVolts", armPIDVolts);
+      double volts = armFeedVolts + armPIDVolts;
+      SmartDashboard.putNumber("ArmTotalVolts", volts);
+      armMotor1.setVoltage(volts);
     }
     public double getArmClampedGoal(double goal) {
       //Find the limits of the arm. Used to move it and ensure that the arm does not move past the amount