   last_time = time_now;
 }
 
+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;
+}
+
 /*
     Loop function
 */
-bool dir;
-uint32_t nextcall = 0;
-const uint16_t intervalperiod = 50*64; // 100 milliseconds
-uint32_t nextcall2 = 1000;
-float vibrationfrequency = 10.0f;
-float amplitude = 1.0f;
-bool on_off = true;
 void loop() {
   // read the position in count
   readPosCount();

*/
void motorControl()
{

  Tp = rp / rs * rh * force;  // Compute the require motor pulley torque (Tp) to generate that force
  // Determine correct direction for motor torque
  if (force < 0) {

unsigned int output = 0;    // output command to the motor
float set_position = 0.0f;

// custom vars
uint32_t nextcall = 0;
const uint16_t intervalperiod = 50*64; // 100 milliseconds
uint32_t nextcall2 = 1000;
float vibrationfrequency = 10.0f;
float amplitude = 1.0f;
bool on_off = true;

/*
      Setup function - this function run once when reset button is pressed.
