 void Set_Servo(int current, int goal) { //Subroutine which takes current position and intended position and turns those into a servo output.
   //(I think this is more efficient/adaptive than writing the same thing out multiple times in Roll_Control()?
 
-  if (current != goal) { //Only run the fin-setting pulse and delay if motion is necessary
+  if (current != goal && millis() - lastTime > 500) { //Only run the fin-setting pulse and delay if motion is necessary
     if (abs(current - goal) == 2)
       digitalWrite(statePinB, HIGH); //If moving from right to left, or vice-versa, pin B needs to be high.
     else

float baroAltBuffer[5] = {0, 0, 0, 0, 0};
float gyroZBuffer[7] = {0, 0, 0, 0, 0, 0, 0}; //The other values are arbitrary; this one is used in Simpsons Rule, so it MUST BE ODD!
float timeBuffer[7] = {0, 0, 0, 0, 0, 0, 0}; //Must equal the size of gyroZBuffer; used for the same calculations.

float accelAverage;
float baroAverage; //Averages of the above buffer values


    else//Otherwise, we are rotating from left to right: clockwise: negative, so A should be high
      digitalWrite(statePinA, HIGH);

    delay(25);

    digitalWrite(controlPin, LOW); //Once the output pins have been set properly, pulse the control pin to update the servo.
    delay(7);
    digitalWrite(controlPin, HIGH);

    finPosition = goal; //Update the fin state tracking variable
    lastTime = millis();
  }

}
