Coral arm

.run/Build.run.xml

@@ -18,6 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>

.run/Robot Deploy Debug .run.xml

@@ -18,6 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>

.run/Robot Deploy.run.xml

@@ -18,6 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>

.run/Simulate Debug.run.xml

@@ -18,6 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>

.run/Simulate.run.xml

@@ -18,6 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>

src/main/java/frc/robot/RobotMap.java

@@ -16,12 +16,14 @@ private RobotMap() {}
     public static final double ARM_CORAL_KI = 0;
     public static final double ARM_CORAL_KD = 0;
     public static final double ARM_CORAL_IZONE = 0;
+    public static final double ARM_CORAL_KF = 0;
     public static final double ARM_CORAL_TOLERANCE_POSITION_DEGREES = 0.5;
     public static final double ARM_CORAL_TOLERANCE_VELOCITY_RPM = 5;
     public static final int ARM_CORAL_START_PULSE_US = 1;
     public static final int ARM_CORAL_END_PULSE_US = 1024;
     public static final double ARM_CORAL_ZERO_OFFSET = 0;
-    public static final double ARM_CORAL_KF = 0;
+    public static final double ARM_CORAL_GEAR_RATIO = 0.005;
+
 
     // coral elevator
     public static final int CORAL_ELEVATOR_UPPER_LIMIT_SWITCH = 1;

src/main/java/frc/robot/commands/CoralArmCommand.java

@@ -1,37 +1,52 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.CoralArm;
 
 public class CoralArmCommand extends Command {
 
     private final CoralArm arm;
+    private final TrapezoidProfile.Constraints constraints;
+
+    private TrapezoidProfile motionProfile;
+    private TrapezoidProfile.State motionProfileGoal;
+    private TrapezoidProfile.State motionProfileSetPoint;
 
     private double targetPositionDegrees;
     private boolean hasNewTarget;
     private boolean didReachPosition;
+    private boolean isHolding;
 
     public CoralArmCommand(CoralArm arm) {
         this.arm = arm;
+        constraints = new TrapezoidProfile.Constraints(1000,200);
 
         addRequirements(arm);
     }
 
     @Override
     public void initialize() {
-
+        isHolding = false;
     }
-
     @Override
     public void execute() {
         if (hasNewTarget) {
             // reset to move to new angle
-
-            didReachPosition = false;
-            hasNewTarget = false;
+            if(!isHolding){
+                arm.stop();
+            }else {
+
+                didReachPosition = false;
+                hasNewTarget = false;
+            }
+            motionProfile = new TrapezoidProfile(constraints);
+            motionProfileGoal = new TrapezoidProfile.State(targetPositionDegrees,0);
+            motionProfileSetPoint = new TrapezoidProfile.State(arm.getPositionDegrees(),0);
         }
-
-        arm.setMoveToPosition(targetPositionDegrees);
+        motionProfileSetPoint = motionProfile.calculate(0.02, motionProfileSetPoint, motionProfileGoal);
+        arm.setMoveToPosition(motionProfileSetPoint.position);
+        //arm.setMoveToPosition(targetPositionDegrees);
         didReachPosition = arm.didReachPosition(targetPositionDegrees);
     }
 
@@ -48,6 +63,7 @@ public boolean isFinished() {
     public void setNewTargetPosition(double positionDegrees) {
         this.targetPositionDegrees = positionDegrees;
         this.hasNewTarget = true;
+        isHolding = true;
     }
 
     public boolean didReachTargetPosition() {
@@ -57,4 +73,7 @@ public boolean didReachTargetPosition() {
 
         return didReachPosition;
     }
+     public void stopHolding(){
+        isHolding = false;
+     }
 }

src/main/java/frc/robot/subsystems/CoralArm.java

@@ -2,8 +2,11 @@
 
 
 import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.*;
+import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.LimitSwitchConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -13,11 +16,13 @@
 public class CoralArm extends SubsystemBase {
 
     private final SparkMax motor;
-    private final AbsoluteEncoder encoder;
+    private final AbsoluteEncoder absEncoder;
+    private final RelativeEncoder relEncoder;
     private final SparkClosedLoopController controller;
     private final SparkLimitSwitch forwardLimitSwitch;
     private final SparkLimitSwitch reverseLimitSwitch;
 
+
     public CoralArm() {
         motor = new SparkMax(RobotMap.ARM_CORAL_MOTOR, SparkLowLevel.MotorType.kBrushless);
         SparkMaxConfig config = new SparkMaxConfig();
@@ -38,23 +43,29 @@ public CoralArm() {
         config.closedLoop
                 .pid(RobotMap.ARM_CORAL_KP, RobotMap.ARM_CORAL_KI, RobotMap.ARM_CORAL_KD)
                 .iZone(RobotMap.ARM_CORAL_IZONE)
-                .outputRange(-1, 1);
+                .outputRange(-1, 1)
+                .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder);
+        config.idleMode(SparkBaseConfig.IdleMode.kBrake);
         motor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+        relEncoder =  motor.getEncoder();
+        absEncoder = motor.getAbsoluteEncoder();
+        relEncoder.setPosition(absEncoder.getPosition());
+
 
-        encoder = motor.getAbsoluteEncoder();
         controller = motor.getClosedLoopController();
         forwardLimitSwitch = motor.getForwardLimitSwitch();
         reverseLimitSwitch = motor.getReverseLimitSwitch();
+
     }
 
     public double getPositionDegrees() {
-        return encoder.getPosition() * 360;
+        return relEncoder.getPosition() * 360;
     }
 
     public boolean didReachPosition(double positionDegrees) {
         double currentPosition = getPositionDegrees();
         return MathUtil.isNear(positionDegrees, currentPosition, RobotMap.ARM_CORAL_TOLERANCE_POSITION_DEGREES) &&
-                Math.abs(encoder.getVelocity()) < RobotMap.ARM_CORAL_TOLERANCE_VELOCITY_RPM;
+                Math.abs(relEncoder.getVelocity()) < RobotMap.ARM_CORAL_TOLERANCE_VELOCITY_RPM;
     }
 
     public boolean isAtForwardLimit() {
@@ -81,7 +92,8 @@ public void stop() {
     public void periodic() {
         SmartDashboard.putBoolean("CoralArmAtForwardLimit", isAtForwardLimit());
         SmartDashboard.putBoolean("CoralArmAtReverseLimit", isAtReverseLimit());
-        SmartDashboard.putNumber("CoralArmPosition", getPositionDegrees());
+        SmartDashboard.putNumber("CoralArmPositionRelative", getPositionDegrees());
+        SmartDashboard.putNumber("CoralArmPositionAbsolute",absEncoder.getPosition());
     }
 
 }