diff --git a/Firmware/src/controller.c b/Firmware/src/controller.c
index 0a8a4b5..0ccdcfc 100644
--- a/Firmware/src/controller.c
+++ b/Firmware/src/controller.c
@@ -1,197 +1,132 @@
 #include "controller.h"
 #include "communication.h"
 #include "drivers/adc.h"
 #include "drivers/dac.h"
 #include "drivers/debug_gpio.h"
 #include "drivers/incr_encoder.h"
 #include "drivers/hall.h"
 #include "drivers/h_bridge.h"
 #include "drivers/callback_timers.h"
 #include "lib/basic_filter.h"
 #include "lib/pid.h"
 #include "lib/utils.h"
 
 extern uint32_t statusReg;
 
 volatile uint32_t  ctrl_timestamp; // [us].
-volatile float32_t ctrl_motorPosHall;
-volatile float32_t ctrl_motorPosCod;
-volatile float32_t ctrl_motorPosCod_c;
-volatile float32_t ctrl_motorPosCodFilt;
-volatile float32_t ctrl_motorTorque_Nm_c; // [N.m].
-volatile uint32_t  ctrl_currentFilterType = FILTER_TYPE_NONE;
-volatile pid_Pid   ctrl_currentPid, ctrl_positionPid;
-volatile bfilt_BasicFilter ctrl_encoderFilter;
+volatile float32_t ctrl_hallVoltage; // [V].
+volatile float32_t ctrl_motorPosCod; // Motor position measured by the incremental encoder [deg].
+volatile float32_t ctrl_motorTorque_Nm_c; // Desired motor torque [N.m].
+volatile pid_Pid   ctrl_currentPid;
 
 bool ctrl_regulateCurrent;
 
 void ctrl_RegulateCurrent(void);
 void ctrl_RegulatePosition(void);
 
 
 /**
   * @brief Initialize the position and current controllers.
   */
 void ctrl_Init(void)
 {
     ctrl_timestamp = 0.0f;
     ctrl_motorTorque_Nm_c = 0.0f;
     
     // By default the current regulator is off, to allow the calibration of the
     // current sensor.
     ctrl_regulateCurrent = false;
     
     // Setup the PIDs.
     pid_Init((pid_Pid*)&ctrl_currentPid, KP_CURRENT_DEFAULT_VAL,
              KI_CURRENT_DEFAULT_VAL, KD_CURRENT_DEFAULT_VAL,
              CURRENT_INTEGRATOR_SAT_DEFAULT_VAL, FF_CURRENT_DEFAULT_VAL);
-    
-#ifdef REGULATION_TYPE_SYMMETRIC
-    pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL,
-             KI_POSITION_DEFAULT_VAL, KD_POSITION_DEFAULT_VAL,
-             POSITION_INTEGRATOR_SAT_DEFAULT_VAL, 0.0f);
-#else
-    pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL,
-             KI_POSITION_DEFAULT_VAL, KD_POSITION_DEFAULT_VAL, 0.0f, 0.0f);
-#endif 
-
-    // Setup the increment encoder filter.
-    bfilt_Init((bfilt_BasicFilter*)&ctrl_encoderFilter, ENCODER_FILT_TAU_DEFAULT_VAL, 0.0f);
-    
+
     // Make the timers call the regulation functions periodically.
     cbt_SetCurrentLoopTimer(ctrl_RegulateCurrent, TE_CURRENT_LOOP_DEFAULT_VAL);
     cbt_SetPositionLoopTimer(ctrl_RegulatePosition, TE_CONTROL_LOOP_DEFAULT_VAL);
     
     // Share some variables with the computer.
     comm_monitorFloat("actual_current [A]", (float32_t*)&ctrl_currentPid.current, READONLY);
     comm_monitorFloat("target_current [A]", (float32_t*)&ctrl_currentPid.target, READONLY);
     
     comm_monitorFloat("current_pid_kp", (float32_t*)&ctrl_currentPid.kp, READWRITE);
     comm_monitorFloat("current_pid_ki", (float32_t*)&ctrl_currentPid.ki, READWRITE);
     comm_monitorFloat("current_pid_kd", (float32_t*)&ctrl_currentPid.kd, READWRITE);
     comm_monitorFloat("current_pid_arw", (float32_t*)&ctrl_currentPid.arw, READWRITE);
     
     comm_monitorFloat("actual_position [deg]", (float32_t*)&ctrl_motorPosCod, READONLY);
-    comm_monitorFloat("target_position [deg]", (float32_t*)&ctrl_motorPosCod_c, READWRITE);
-    
-    comm_monitorFloat("position_pid_kp", (float32_t*)&ctrl_positionPid.kp, READWRITE);
-    comm_monitorFloat("position_pid_ki", (float32_t*)&ctrl_positionPid.ki, READWRITE);
-    comm_monitorFloat("position_pid_kd", (float32_t*)&ctrl_positionPid.kd, READWRITE);
-    comm_monitorFloat("position_pid_arw", (float32_t*)&ctrl_positionPid.arw, READWRITE);
     
     comm_monitorFloat("motor_torque [N.m]", (float32_t*)&ctrl_motorTorque_Nm_c, READWRITE);
     comm_monitorFloat("encoder_pos [deg]", (float32_t*)&ctrl_motorPosCod, READONLY);
-    comm_monitorFloat("hall_raw_pos [deg]", (float32_t*)&ctrl_motorPosHall, READONLY);
+    comm_monitorFloat("hall_voltage [V]", (float32_t*)&ctrl_hallVoltage, READONLY);
 }
 
 /**
   * @brief Start the current regulation.
   */
 void ctrl_StartCurrentLoop(void)
 {
     ctrl_regulateCurrent = true;
 }
 
 /**
   * @brief  Current regulation "loop" function.
   */
 void ctrl_RegulateCurrent()
 {
     float32_t motorVoltage, // Motor command voltage [V].
               pwmNormalizedDutyCycle, // Motor normalized PWM duty (-1 ot 1).
               targetCurrent; // [A].
     float32_t motorCurrentCurrent; // [A].
              
-    //
-    dio_Set(2, true);
-    
     // Get the current.
     motorCurrentCurrent = adc_GetCurrent();
     
     // Convert the target torque to current.
     targetCurrent = ctrl_motorTorque_Nm_c / MOTOR_TORQUE_CONST;
     
     // Regulate.
     motorVoltage = -pid_Step((pid_Pid*)&ctrl_currentPid,
                              motorCurrentCurrent,
                              targetCurrent,
                              (float32_t)cbt_GetCurrentLoopPeriod()*MICROSECOND_TO_SECOND);
     
     // Normalize to get a signed PWM duty (between -1 and 1).
     pwmNormalizedDutyCycle = motorVoltage / H_BRIDGE_SUPPLY_VOLTAGE;
     utils_SaturateF(&pwmNormalizedDutyCycle, -CURRENT_LOOP_PWM_MAX_DUTY_CYCLE, CURRENT_LOOP_PWM_MAX_DUTY_CYCLE);
 
-    // Apply the computed PWM duty, if the enabled.
+    // Apply the computed PWM duty, if the current regulation is enabled.
     if(ctrl_regulateCurrent)
         hb_SetPWM(pwmNormalizedDutyCycle);
     else
         hb_SetPWM(0.0f);
-
-    //
-    dio_Set(2, false);
 }
 
 
 
 /**
   * @brief Position regulation "loop" function.
   * @note As an example, a basic position regulator that uses the encoder is
   * implemented. Feel free to remove all the content of the function to add your
   * own code.
   */
 void ctrl_RegulatePosition()
 {
-    //
-    dio_Set(1, true);
-    
     // Increment the timestamp.
     ctrl_timestamp += cbt_GetPositionLoopPeriod();
     
     // Get the Hall sensor position.
-    ctrl_motorPosHall = hall_Get();
+    ctrl_hallVoltage = hall_Get();
 
     // Get the encoder position.
     ctrl_motorPosCod = enc_GetPosition();
 
-    // Filter the position given by the encoder.
-    if(ctrl_currentFilterType == FILTER_TYPE_FIRST_ORDER)
-    {
-        ctrl_motorPosCodFilt = bfilt_Step((bfilt_BasicFilter*)&ctrl_encoderFilter, ctrl_motorPosCod);
-    }
-    else if(ctrl_currentFilterType == FILTER_TYPE_RUNNING_MEAN)
-    {
-        // TODO.
-        ctrl_motorPosCodFilt = ctrl_motorPosCod;
-    }
-    else // No filter.
-        ctrl_motorPosCodFilt = ctrl_motorPosCod;
-
-    // Regulate.
-/*#ifdef REGULATION_TYPE_SYMMETRIC // No wall, just regulation on a point.
-
-    ctrl_motorTorque_Nm_c = -pid_Step((pid_Pid*)&ctrl_positionPid,
-                                     ctrl_motorPosCodFilt,
-                                     ctrl_motorPosCod_c,
-                                     (float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND);
-
-#else // Wall, regulate only if not touching the wall.
-
-    if(ctrl_motorPosCodFilt < ctrl_motorPosCod_c)  // Touching the wall.
-    {
-        ctrl_motorTorque_Nm_c = -pid_Step((pid_Pid*)&ctrl_positionPid,
-                                         ctrl_motorPosCodFilt,
-                                         (float32_t)ctrl_motorPosCod_c,
-                                         (float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND);
-    }
-    else
-        ctrl_motorTorque_Nm_c = 0.0f;
-#endif*/
+    // Compute the motor torque.
+    ctrl_motorTorque_Nm_c = 0.0f;
     
     // Saturate the torque to the motor nominal.
     utils_SaturateF((float32_t*)&ctrl_motorTorque_Nm_c, -MOTOR_NOMINAL_TORQUE,
                     MOTOR_NOMINAL_TORQUE);
-    
-    //                
-    dio_Set(1, false);
 }
 
diff --git a/Firmware/src/drivers/led.c b/Firmware/src/drivers/led.c
index 33aa85a..79373d9 100644
--- a/Firmware/src/drivers/led.c
+++ b/Firmware/src/drivers/led.c
@@ -1,108 +1,121 @@
 #include "led.h"
 #include "../communication.h"
 
 #define N_LEDS 4
 
 #define LED_TIMER TIM1
 #define LED_PORT GPIOA
 #define LED_MAX_DUTY 255
 #define LED_PWM_FREQ 32000 // [Hz].
 
 typedef struct
 {
     uint16_t const pin, pinSource;
     volatile uint32_t *const duty;
 } led_Led;
 
 led_Led led_leds[N_LEDS] =
 {
     { GPIO_Pin_8, GPIO_PinSource8, &LED_TIMER->CCR1 },
     { GPIO_Pin_9, GPIO_PinSource9, &LED_TIMER->CCR2 },
     { GPIO_Pin_10, GPIO_PinSource10, &LED_TIMER->CCR3 },
     { GPIO_Pin_11, GPIO_PinSource11, &LED_TIMER->CCR4 },
 };
 
 void setLed0(float32_t brightness) { led_Set(0, brightness); };
 void setLed1(float32_t brightness) { led_Set(1, brightness); };
 void setLed2(float32_t brightness) { led_Set(2, brightness); };
 void setLed3(float32_t brightness) { led_Set(3, brightness); };
 
 float32_t getLed0(void) { return led_Get(0); };
 float32_t getLed1(void) { return led_Get(1); };
 float32_t getLed2(void) { return led_Get(2); };
 float32_t getLed3(void) { return led_Get(3); };
 
+/**
+ * @brief Initializes the LEDs module.
+ */
 void led_Init(void)
 {
     int i;
     GPIO_InitTypeDef GPIO_InitStruct;
     TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
     TIM_OCInitTypeDef TIM_OCInitStruct;
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
     
     // Initialize each pin.
     for(i=0; i<N_LEDS; i++)
     {
         GPIO_InitStruct.GPIO_Pin   = led_leds[i].pin;
         GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_AF;
         GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
         GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
         GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_NOPULL;
         GPIO_Init(LED_PORT, &GPIO_InitStruct);
         
         GPIO_PinAFConfig(LED_PORT, led_leds[i].pinSource, GPIO_AF_TIM1);
     }
     
     // Initialize the timer peripheral, as PWM generator.
     TIM_TimeBaseStruct.TIM_Period = LED_MAX_DUTY;  
     TIM_TimeBaseStruct.TIM_Prescaler = (STM_SYSCLOCK_FREQ / APB2_PRESCALER * 2) / LED_PWM_FREQ;
     TIM_TimeBaseStruct.TIM_ClockDivision = 0;
     TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up;
     TIM_TimeBaseStruct.TIM_RepetitionCounter = 0;
     TIM_TimeBaseInit(LED_TIMER, &TIM_TimeBaseStruct);
     
     TIM_OCInitStruct.TIM_OCMode      = TIM_OCMode_PWM2;
     TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
     TIM_OCInitStruct.TIM_Pulse       = 0;
     TIM_OCInitStruct.TIM_OCPolarity  = TIM_OCPolarity_Low;
     TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Reset;
     
     TIM_OC1Init(LED_TIMER, &TIM_OCInitStruct);
     TIM_OC2Init(LED_TIMER, &TIM_OCInitStruct);
     TIM_OC3Init(LED_TIMER, &TIM_OCInitStruct);
     TIM_OC4Init(LED_TIMER, &TIM_OCInitStruct);
     
     TIM_OC1PreloadConfig(LED_TIMER, TIM_OCPreload_Enable);
     TIM_OC2PreloadConfig(LED_TIMER, TIM_OCPreload_Enable);
     TIM_OC3PreloadConfig(LED_TIMER, TIM_OCPreload_Enable);
     TIM_OC4PreloadConfig(LED_TIMER, TIM_OCPreload_Enable);
 
     TIM_SelectOutputTrigger(LED_TIMER, TIM_TRGOSource_Update);
 
     TIM_Cmd(LED_TIMER, ENABLE);
     TIM_CtrlPWMOutputs(LED_TIMER, ENABLE);
     
     // Create the SyncVars.
-    comm_monitorFloatFunc("led_0", getLed0, setLed0);
-    comm_monitorFloatFunc("led_1", getLed1, setLed1);
-    comm_monitorFloatFunc("led_2", getLed2, setLed2);
-    comm_monitorFloatFunc("led_3", getLed3, setLed3);
+    comm_monitorFloatFunc("led_0 [0.0-1.0]", getLed0, setLed0);
+    comm_monitorFloatFunc("led_1 [0.0-1.0]", getLed1, setLed1);
+    comm_monitorFloatFunc("led_2 [0.0-1.0]", getLed2, setLed2);
+    comm_monitorFloatFunc("led_3 [0.0-1.0]", getLed3, setLed3);
 }
 
+/**
+ * @brief Gets the intensity of a single LED.
+ * @param ledIndex: LED index (0, 1, 2 or 3).
+ * @return the current LED intensity [0.0-1.0].
+ */
 float32_t led_Get(int ledIndex)
 {
     if(ledIndex >= 0 && ledIndex < N_LEDS)
         return ((float32_t)(*led_leds[ledIndex].duty)) / (float32_t)LED_MAX_DUTY;
     else
         return 0.0f;
 }
 
+/**
+ * @brief Sets the intensity of a single LED.
+ * @param ledIndex: LED index (0, 1, 2 or 3).
+ * @param brightness: the LED intensity [0.0-1.0].
+ */
 void led_Set(int ledIndex, float32_t brightness)
 {
     if(ledIndex >= 0 && ledIndex < N_LEDS &&
        brightness >= 0.0f && brightness <= 1.0f)
     {
         *led_leds[ledIndex].duty = (uint32_t)(brightness * (float32_t)LED_MAX_DUTY);
     }
 }
diff --git a/Firmware/src/main.c b/Firmware/src/main.c
index 72f3ca2..6716ade 100644
--- a/Firmware/src/main.c
+++ b/Firmware/src/main.c
@@ -1,100 +1,76 @@
 #include "main.h"
 #include "communication.h"
 #include "controller.h"
 #include "drivers/adc.h"
 #include "drivers/callback_timers.h"
 #include "drivers/dac.h"
 #include "drivers/debug_gpio.h"
 #include "drivers/h_bridge.h"
 #include "drivers/hall.h"
 #include "drivers/incr_encoder.h"
 #include "drivers/led.h"
 #include "drivers/strain_gauge.h"
 #include "drivers/tachometer.h"
 #include "lib/utils.h"
 
 extern volatile uint32_t ctrl_timestamp;
 
 volatile uint32_t statusReg = 0x00000000; // Main state register.
 
 /**
   * @brief  Main function, setups all the drivers and controllers.
   */
 int main(void)
 {
-    float demoSineOutput, demoCosineOutput, demoSineAmplitude;
-    int16_t testInt;
-    
     // Setup the GPIOs and the interrupts.
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB |
                            RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD |
                            RCC_AHB1Periph_GPIOE, ENABLE);
     NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
 
     //
     cbt_Init(); // Setup the timers that will call the loops functions.
     
 	adc_Init(); // Setup the ADC.
 	
 	comm_Init(); // Setup the communication module.
     
     ctrl_Init(); // Setup the controllers.
 	
     enc_Init(); // Setup the incremental encoders.		
     
     hb_Init();   // Setup the H-bridge.
     hb_Enable(); //
     
     dac_Init(); // Setup the DAC.
     
-    //led_Init();
+    led_Init();
+    
     dio_Init();
     
     // Delay to let the power electronics stabilize before calibrating the
     // current sensor.
     utils_DelayMs(200);
     
     adc_CalibrateCurrentSens();
     
     // Start the current loop.
     ctrl_StartCurrentLoop();
     
     // Init the external sensors drivers, connected to the ADC.
     hall_Init(ANIN1);
     //sg_Init(ANIN2);
     //tac_Init(ANIN2);
     
-    // SyncVar demo with a configurable sine wave.
-    testInt = 0.0f;
-    demoSineAmplitude = 1.0f;
-    comm_monitorFloat("demo_sine_output", &demoSineOutput, READONLY);
-    comm_monitorFloat("demo_cosine_output", &demoCosineOutput, READONLY);
-    comm_monitorFloat("demo_sine_amplitude", &demoSineAmplitude, READWRITE);
-    comm_monitorInt16("demo_test_int", &testInt, READWRITE);
-    
     // End of the initialization. Lock the SyncVar list, and notify the PC that
     // the board has (re)started.
     comm_LockSyncVarsList();
     comm_NotifyReady();
 
 	// Endless loop. The low priority functions are called here.
 	while(1)
 	{
-        //
-        dio_Set(0, true);
-        
 		// Update the communication.
         comm_Step();
-        
-        // SyncVar demo with a configurable sine wave.
-        demoSineOutput = demoSineAmplitude *
-                         sinf(2.0f*3.14f*((float)ctrl_timestamp) / 1000000.0f
-                                          * 0.2f);
-        demoCosineOutput = demoSineAmplitude *
-                         cosf(2.0f*3.14f*((float)ctrl_timestamp) / 1000000.0f
-                                          * 0.2f);
-  
-        //
-        dio_Set(0, false);
 	}
 }