 double rh = 0.065659;   //[m]
 // Force output variables
 double force = 0;           // force at the handle
+double force_vibrate = 0;
 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

  static float last_error = 0.0f;
  static float I = 0.0f;

  static float kP = 0.2f;
  static float kI = 0.0f;
  static float kD = 0.00004f;

  const float MAX_DIFFERENCE = 0.0f;
  const float LIMITPLUS = 1.0f;

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.
