 float vibrationfrequency = 10.0f;
 float amplitude = 1.0f;
 bool on_off = true;
+float springfactor = 0.0f;
 
 /*
       Setup function - this function run once when reset button is pressed.

double rh = 0.065659;   //[m]
// Force output variables
double force = 0;           // force at the handle
double Tp = 0;              // torque of the motor pulley
double duty = 0;            // duty cylce (between 0 and 255)
unsigned int output = 0;    // output command to the motor

  const float MAX_FORCE_OUTPUT = 0.0f;
  float actualForceUsed = 0.0f;

  kP_scaled = kP * springfactor / 1000.0f;

  float error = set_position - xh;

  float P = error * kP_scaled;

  float D = -vh * kD;

  force = P + I + D;
}

void shake(int interval_ms,float frequency_hz, float strength)
{
    vibrationfrequency = frequency_hz;
    nextcall = millis() + (uint32_t)interval_ms * 64UL;
    amplitude = strength;
    on_off = true;
}
