package org.team2471.bunnybot.commands;

import org.team2471.frc.lib.motion_profiling.MotionProfileAnimation;
import org.team2471.frc.lib.motion_profiling.MotionProfileCurve;
import org.team2471.frc.lib.motion_profiling.PlayAnimationCommand;

import static org.team2471.bunnybot.Robot.arm;

public class ReadyToSpitCommand extends PlayAnimationCommand{

  MotionProfileAnimation animation;
  MotionProfileCurve shoulderCurve;
  MotionProfileCurve elbowCurve;

  public ReadyToSpitCommand() {
    requires(arm);

    animation = new MotionProfileAnimation();
    setAnimation(animation);
    shoulderCurve = new MotionProfileCurve( arm.shoulderController );
    elbowCurve = new MotionProfileCurve( arm.elbowController );
    animation.addMotionProfileCurve( shoulderCurve );
    animation.addMotionProfileCurve( elbowCurve );

    shoulderCurve.storeValue( 0.0, 51.0 );
    shoulderCurve.storeValue( 1.0, 26.0 );

    elbowCurve.storeValue( 0.0, -113.0 );
    elbowCurve.storeValue( 1.0, -63.0 );
  }

  @Override
  protected void initialize() {
    if (arm.shoulderEncoder.pidGet() > 40) {
      setSpeed(1.0);
    }
    else {
      setSpeed(-1.0);
    }
    System.out.println("ready to spit initialized");
  }
}

@@ -859,24 +859,68 @@
 .0, 51.0 );%0A
+    shoulderCurve.storeValue( 0.75, 44.0 );%0A
     shoulder
@@ -989,24 +989,66 @@
 , -113.0 );%0A
+    elbowCurve.storeValue( 0.75, -70.0 );%0A
     elbowCur
@@ -1240,61 +1240,8 @@
   %7D%0A
-    System.out.println(%22ready to spit initialized%22);%0A
   %7D%0A

add an extra key so that the intake doesn't hit the can on the way up/down.