   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) {

    //Serial.print("force: ");
    //Serial.println(force);
  } else if (fbEvent.startsWith("HitCar")) {
	shake(500, 100.0f, 2.0f);
  } else if (fbEvent.startsWith("Creak")) {
    shake(500, 100.0f, 2.0f);
  } else if (fbEvent.startsWith("Smash")) {
	shake(500, 100.0f, 2.0f);
  } else if (fbEvent.startsWith("Wreck")) {
	shake(500, 200.0f, 2.0f);
  } else if (fbEvent.startsWith("Offroad")) {
	shake(500, 50.0f, 2.0f);
  } else if (fbEvent.startsWith("Grounded")) {
	shake(200, 500.0f, 4.0f);
  }
}

