diff --git a/Firmware/src/haptic_controller.c b/Firmware/src/haptic_controller.c
index dc408b1..c681c4f 100644
--- a/Firmware/src/haptic_controller.c
+++ b/Firmware/src/haptic_controller.c
@@ -1,75 +1,78 @@
 /*
  * Copyright (C) 2017 EPFL-LSRO (Laboratoire de Systemes Robotiques).
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
 
 #include "haptic_controller.h"
 #include "communication.h"
 #include "drivers/adc.h"
 #include "drivers/incr_encoder.h"
 #include "drivers/hall.h"
 #include "drivers/callback_timers.h"
 #include "lib/utils.h"
 #include "torque_regulator.h"
 
-#define CONTROL_LOOP_PERIOD 350 // Main control loop period [us].
+#define DEFAULT_HAPTIC_CONTROLLER_PERIOD 350 // Default control loop period [us].
 
 volatile uint32_t  hapt_timestamp; // Time base of the controller, also used to timestamp the samples sent by streaming [us].
 volatile float32_t hapt_hallVoltage; // Hall sensor output voltage [V].
 volatile float32_t hapt_encoderPaddleAngle; // Paddle angle measured by the incremental encoder [deg].
 volatile float32_t hapt_motorTorque; // Motor torque [N.m].
 
 void hapt_Update(void);
 
 /**
   * @brief Initializes the haptic controller.
   */
 void hapt_Init(void)
 {
-    hapt_timestamp = 0.0f;
+    hapt_timestamp = 0;
+    hapt_motorTorque = 0.0f;
 
     // Make the timers call the update function periodically.
-    cbt_SetPositionLoopTimer(hapt_Update, CONTROL_LOOP_PERIOD);
-    
+    cbt_SetHapticControllerTimer(hapt_Update, DEFAULT_HAPTIC_CONTROLLER_PERIOD);
+
     // Share some variables with the computer.
+    comm_monitorUint32Func("timestep [us]", cbt_GetHapticControllerPeriod,
+                           cbt_SetHapticControllerPeriod);
     comm_monitorFloat("motor_torque [N.m]", (float32_t*)&hapt_motorTorque, READWRITE);
     comm_monitorFloat("encoder_paddle_pos [deg]", (float32_t*)&hapt_encoderPaddleAngle, READONLY);
     comm_monitorFloat("hall_voltage [V]", (float32_t*)&hapt_hallVoltage, READONLY);
 }
 
 /**
   * @brief Updates the haptic controller state.
   */
 void hapt_Update()
 {
     float32_t motorShaftAngle; // [deg].
 
     // Compute the dt (uncomment if you need it).
-    //float32_t dt = ((float32_t)cbt_GetPositionLoopPeriod()) / 1000000.0f; // [s].
+    //float32_t dt = ((float32_t)cbt_GetHapticControllerPeriod()) / 1000000.0f; // [s].
 
     // Increment the timestamp.
-    hapt_timestamp += cbt_GetPositionLoopPeriod();
+    hapt_timestamp += cbt_GetHapticControllerPeriod();
     
     // Get the Hall sensor voltage.
     hapt_hallVoltage = hall_GetVoltage();
 
     // Get the encoder position.
     motorShaftAngle = enc_GetPosition();
     hapt_encoderPaddleAngle = motorShaftAngle / REDUCTION_RATIO;
 
     // Compute the motor torque, and apply it.
     //hapt_motorTorque = 0.0f;
     torq_SetTorque(hapt_motorTorque);
 }