added more ath

src/main/java/org/carlmontrobotics/Constants.java

@@ -19,7 +19,8 @@ public static final class Drivetrain {
 	}
 
 	public static final class Arm {
-
+		//Motor port
+		public final static int MOTOR_PORT = 8;
 		//all angles in rot here
 		public static final double intakeAngle = -.1;
 		public static final double ampAngle = .3;

src/main/java/org/carlmontrobotics/subsystems/Arm.java

@@ -4,9 +4,18 @@
 
 package org.carlmontrobotics.subsystems;
 
+import org.carlmontrobotics.Constants;
 import org.carlmontrobotics.Constants.Arm.*;
+import org.carlmontrobotics.lib199.MotorConfig;
+import org.carlmontrobotics.lib199.MotorControllerFactory;
 
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -15,7 +24,10 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 public class Arm extends SubsystemBase {
-    public Arm() {
+    private final CANSparkMax armMotor = MotorControllerFactory.createSparkMax(Constants.Arm.MOTOR_PORT,MotorConfig.NEO);
+    private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
+    private final XboxController controller;
+    public Arm(XboxController controller) {
 			//arm 
       /*
        have 3 set positions
@@ -26,23 +38,27 @@ public Arm() {
 
        There will also be a manual control for the arm using the right joystick
       */
+      this.controller = controller;
     }
 
-    public void setArmPos() {
+    public void setArmPos(double targetPosition, double targetVelocity) {
       //Sets arm to the optimal angle for amp, speaker and Ground intake | used to score in amp
       //these values are in constants
       //pass in where scorign and use switch statement
-
 
+
     }
 
-    public void setSpeed() {
+    public void controllerSetSpeed(double rightJoystick) {
       //move the arm around based off the right joystick movement on the manipulator joystick
       //use the trapezoid thingy from robot code 2023
     }
-
+    public double getArmClampedGoal(double goal) {
+      return MathUtil.clamp(MathUtil.inputModulus(goal, goal, goal), goal, goal);
+    }
     @Override
     public void periodic() {
      // run moveArm passing in controller here
+     controllerSetSpeed(controller.getRightY());
 		}
 }