diff --git a/Firmware/src/controller.h b/Firmware/src/controller.h
index 295a004..3d56639 100644
--- a/Firmware/src/controller.h
+++ b/Firmware/src/controller.h
@@ -1,62 +1,62 @@
 #ifndef __CONTROLLER_H
 #define __CONTROLLER_H
 
 #include "main.h"
 
 // Type of regulation.
 // Comment out to get the "wall" non-symetric behavior.
 #define REGULATION_TYPE_SYMMETRIC
 
 // Loops periode values
 #define TE_LOOP_MIN_VALUE 50    // Minimum period for any loop [us].
 #define TE_LOOP_MAX_VALUE 65534 // Maximum period for any loop [us].
 
-#define TE_CURRENT_LOOP_DEFAULT_VAL 30   // Current control loop period [us] (max 2^16-1) (default value at reset).
+#define TE_CURRENT_LOOP_DEFAULT_VAL 50   // Current control loop period [us] (max 2^16-1) (default value at reset).
 #define TE_CONTROL_LOOP_DEFAULT_VAL 350  // Main control loop period [us] (max 2^16-1) (default value at reset).
 #define TE_DATA_LOOP_DEFAULT_VAL    8000 // Data loop period [us] (max 2^16-1) (default value at reset).
 
 // Current loop parameters
-#define KP_CURRENT_DEFAULT_VAL 0.5f		
-#define KI_CURRENT_DEFAULT_VAL 10.0f
+#define KP_CURRENT_DEFAULT_VAL 5.0f		
+#define KI_CURRENT_DEFAULT_VAL 0.0f
 #define FF_CURRENT_DEFAULT_VAL MOTOR_RESISTANCE
 #define CURRENT_INTEGRATOR_SAT          2.0f
 #define CURRENT_LOOP_PWM_MAX_DUTY_CYCLE	0.98f // Max PWM value (normalized) duty cycle (less than 1 to ensure bootstrap capacitor charging)
 
 // Main control loop parameters
 #define KP_POSITION_DEFAULT_VAL 0.01f
 #define KD_POSITION_DEFAULT_VAL 0.0001f
 #define KI_POSITION_DEFAULT_VAL 0.0f
 #define POSITION_INTEGRATOR_SAT_DEFAULT_VAL 0.01f
 
 // Increment encoder filter default coef.
 #define ENCODER_FILT_TAU_DEFAULT_VAL 0.999f
 
 // Filter types
 #define FILTER_TYPE_NONE         0
 #define FILTER_TYPE_FIRST_ORDER  1
 #define FILTER_TYPE_RUNNING_MEAN 2
 
 /** @defgroup Controller Main / Controllers
   * @brief Control the paddle
   *
   * This module is the high-level controller of the board. It runs two "loops"
   * (in fact, functions called periodically by the interrupt of a timer), one to
   * regulate the motor current (ctrl_RegulateCurrent(), high frequency) and
   * another to control the rest (ctrl_RegulatePosition(), lower frequency).
   * The content of ctrl_RegulatePosition() is typically what the user of the
   * board will modify, depending on the selected sensors and control algorithms.
   *
   * Call ctrl_Init() to setup this module. Its interrupt functions will be called
   * automatically periodically.
   *
   * @addtogroup Controller
   * @{
   */
 
 void ctrl_Init(void);
 
 /**
   * @}
   */
 
 #endif
diff --git a/Firmware/src/drivers/adc.c b/Firmware/src/drivers/adc.c
index 582231f..58d6bdc 100644
--- a/Firmware/src/drivers/adc.c
+++ b/Firmware/src/drivers/adc.c
@@ -1,233 +1,233 @@
 #include "adc.h"
 #include "../lib/basic_filter.h"
 #include "../lib/utils.h"
 
 volatile uint16_t adc_currentValuesBuffer[ADC_BUFFER_SIZE];
 bfilt_BasicFilter adc_currentFilter;
 float32_t adc_currentSensOffset = 0.0f;
 
 void adc_DmaInit(void);
 
 /**
   * @brief  Initialize the ADC converter (2 analog inputs + current sense).
   */
 void adc_Init(void)
 {
     ADC_InitTypeDef         ADC_InitStructure;
     ADC_CommonInitTypeDef   ADC_CommonInitStructure;
     GPIO_InitTypeDef        GPIO_InitStructure;
         
     adc_DmaInit();
 
     // Periph clock enable
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1|RCC_APB2Periph_ADC2, ENABLE);
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC3,ENABLE);
         
     // GPIO configuration
     GPIO_InitStructure.GPIO_Pin  = ADC1_Input_Pin;
     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
     GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
     GPIO_Init(ADC1_Input_Port, &GPIO_InitStructure);
     GPIO_InitStructure.GPIO_Pin = ADC2_Input_Pin;
     GPIO_Init(ADC2_Input_Port, &GPIO_InitStructure);
 
     GPIO_InitStructure.GPIO_Pin = ADC_Current_Sens_Pin;
     GPIO_Init(ADC_Current_Sens_Port, &GPIO_InitStructure);
 
     // ADC Common configuration
     ADC_DeInit();
     ADC_CommonInitStructure.ADC_Mode             = ADC_Mode_Independent;
     ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; // Useless here (only used in dual or triple interleaved modes).
     ADC_CommonInitStructure.ADC_DMAAccessMode    = ADC_DMAAccessMode_Disabled;
     ADC_CommonInitStructure.ADC_Prescaler        = ADC_Prescaler_Div4;
     ADC_CommonInit(&ADC_CommonInitStructure);
 
     // ADC regular channel configuration
     ADC_InitStructure.ADC_Resolution           = ADC_Resolution_12b;
     ADC_InitStructure.ADC_ScanConvMode         = DISABLE;
     ADC_InitStructure.ADC_ContinuousConvMode   = DISABLE;
     ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
     ADC_InitStructure.ADC_ExternalTrigConv     = 0;
     ADC_InitStructure.ADC_DataAlign            = ADC_DataAlign_Right;
     ADC_InitStructure.ADC_NbrOfConversion      = 1;
     ADC_Init(ADC1, &ADC_InitStructure);
 
     ADC_Init(ADC2, &ADC_InitStructure); 
 //    ADC_InitStructure.ADC_ExternalTrigConvEdge        = ADC_ExternalTrigConvEdge_Rising;
 //      ADC_InitStructure.ADC_ExternalTrigConv              = ADC_ExternalTrigConv_T8_TRGO;
 //    ADC_InitStructure.ADC_ContinuousConvMode          = DISABLE;
     // Synchronous sampling with the PWM replaced by a high-frequency sampling at ~300kHz (over-sampling), because of the commutation noise of the current measuring amplifier at ~120kHz.
     ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;       
     ADC_Init(ADC3, &ADC_InitStructure);   
 
     ADC_RegularChannelConfig(ADC1, ADC1_Input_Chan, 1, ADC_SampleTime_56Cycles);
     ADC_Cmd(ADC1, ENABLE);
     ADC_SoftwareStartConv(ADC1);
 
     ADC_RegularChannelConfig(ADC2, ADC2_Input_Chan, 1, ADC_SampleTime_56Cycles);
     ADC_Cmd(ADC2, ENABLE);
     ADC_SoftwareStartConv(ADC2);
     
     ADC_RegularChannelConfig(ADC3, ADC_Current_Sens_Chan, 1, ADC_SampleTime_28Cycles);
     ADC_DMARequestAfterLastTransferCmd(ADC3, ENABLE);
     ADC_DMACmd(ADC3, ENABLE);
     ADC_Cmd(ADC3, ENABLE);
     ADC_SoftwareStartConv(ADC3);
 
     // Setup the filter of the ADC current samples over time.
-    bfilt_Init(&adc_currentFilter, 0.2f, 0.0f);
+    bfilt_Init(&adc_currentFilter, 0.05f, 0.0f);
 }
 
 /**
   * @brief  Setup the DMA that copies the current ADC samples to the RAM.
   */
 void adc_DmaInit(void)
 {
     DMA_InitTypeDef DMA_InitStructure;
 
     RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
     
     DMA_InitStructure.DMA_Memory0BaseAddr    = (uint32_t)&adc_currentValuesBuffer;
     DMA_InitStructure.DMA_DIR                = DMA_DIR_PeripheralToMemory;
     DMA_InitStructure.DMA_BufferSize         = ADC_BUFFER_SIZE;
     DMA_InitStructure.DMA_PeripheralInc      = DMA_PeripheralInc_Disable;
     DMA_InitStructure.DMA_MemoryInc          = DMA_MemoryInc_Enable;
     DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
     DMA_InitStructure.DMA_MemoryDataSize     = DMA_MemoryDataSize_HalfWord;
     DMA_InitStructure.DMA_Mode               = DMA_Mode_Circular;
     DMA_InitStructure.DMA_Priority           = DMA_Priority_High;
     DMA_InitStructure.DMA_FIFOMode           = DMA_FIFOMode_Enable;
     DMA_InitStructure.DMA_FIFOThreshold      = DMA_FIFOThreshold_HalfFull;
     DMA_InitStructure.DMA_MemoryBurst        = DMA_MemoryBurst_Single;
     DMA_InitStructure.DMA_PeripheralBurst    = DMA_PeripheralBurst_Single;
 
     DMA_InitStructure.DMA_Channel            = DMA_Channel_2;
     DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC3->DR;
     DMA_Init(DMA2_Stream1, &DMA_InitStructure);
     DMA_Cmd(DMA2_Stream1, ENABLE);
 
 }
 
 
 /**
   * @brief  Compute the current sense offset.
   * @note   Run before enabling current regulation.
   */
 void adc_CalibrateCurrentSens(void)
 {
     float32_t offset;
     int32_t i=0;
     
     for(i=0; i<ADC_CALIB_N_SAMPLES; i++)
     {
         offset += adc_GetCurrent() / (float32_t)ADC_CALIB_N_SAMPLES;
     
         utils_DelayUs(400);
     }
         
     adc_currentSensOffset = offset;
 }
 
 /**
   * @brief  Compute the current sense offset.
   * @retval The measured current in [mA].
   */
 float32_t adc_GetCurrent(void)
 {
     int32_t motorCurrentSum;
     float32_t motorCurrentMean;
 
     int32_t i;
     
     // Compute the average of all the ADC values in the buffer.
     motorCurrentSum = 0; // [ADC raw value].
 
     for(i=0; i<ADC_BUFFER_SIZE; i++)
         motorCurrentSum += (int32_t)(adc_currentValuesBuffer[i]);
     
     motorCurrentMean = (float32_t)motorCurrentSum / (float32_t)ADC_BUFFER_SIZE;
 
     // Convert from ADC increments to a current in [A].
     motorCurrentMean *= ADC_CURRENT_SCALE;
     
     // Add the offset obtained during the sense resistor calibration.
     motorCurrentMean -= (float32_t) adc_currentSensOffset;
     
     // Use low-pass filtering to reduce the noise.
     bfilt_Step(&adc_currentFilter, motorCurrentMean);
     
     //return motorCurrentMean;
     return adc_currentFilter.filteredValue;
 }
 
 /**
   * @brief  Start the ADC conversion of the selected channel.
   */
 void adc_StartConversion(AdcChannel channel)
 {
     if(channel == ANIN1)
         ADC1->CR2 |= (uint32_t)ADC_CR2_SWSTART;
     else if(channel == ANIN2)
         ADC2->CR2 |= (uint32_t)ADC_CR2_SWSTART;
 }
 
 /**
   * @brief  Get the ADC conversion status of the selected channel.
   * @retval 1 if the conversion is finished, 0 otherwise.
   */
 uint8_t adc_ConversionIsFinished(AdcChannel channel)
 {
     if(channel == ANIN1)
         return (ADC1->SR & ADC_SR_EOC) != 0;
     else if(channel == ANIN2)
         return (ADC2->SR & ADC_SR_EOC) != 0;
     else
         return 0;
 }
 
 /**
   * @brief  Get the voltage measured by the selected ADC channel.
   * @param  channel: the channel of the ADC.
   * @param  scale: the full scale, depends of the switches position.
   * @return the measured voltage, in [V], or 0.0f if the channel is invalid.
   * @note   make sure that the conversion is finished before calling.
   */
 float32_t adc_GetConversionResult(AdcChannel channel, float32_t scale)
 {
     float32_t adcRawVal, voltage;
     
     if(channel == ANIN1)
         adcRawVal = (float32_t)ADC1->DR;
     else if(channel == ANIN2)
         adcRawVal = (float32_t)ADC2->DR;
     else
         adcRawVal = 0.0f;
         
     voltage = (1.0f - (adcRawVal / (float32_t)ADC_MAX) * 2.0f) * scale;
  
     return voltage;
 }
 
 /**
   * @brief  Get the voltage measured by the selected ADC channel.
   * @param  channel: the channel of the ADC.
   * @param  scale: the full scale, depends of the switches position.
   * @return the measured voltage, in [V], or 0.0f if the channel is invalid.
   * @note   make sure that the conversion is finished before calling.
   */
 float32_t adc_GetChannelVoltage(AdcChannel channel, float32_t scale)
 {
     int32_t conversionTime = 0;
     
     adc_StartConversion(channel);
     
     while(!adc_ConversionIsFinished(channel))
     {
         if(conversionTime < ADC_MAX_CONVERSION_TIME)
             conversionTime++;
         else
             break;
     }
 
     return adc_GetConversionResult(channel, scale);
 }   
diff --git a/Firmware/src/lib/utils.c b/Firmware/src/lib/utils.c
index 58ce991..3a1a5f5 100644
--- a/Firmware/src/lib/utils.c
+++ b/Firmware/src/lib/utils.c
@@ -1,87 +1,87 @@
 #include "utils.h"
 
 /**
   * @brief  "Busy wait" delay function 
   * @param  duration: Delay time in [us] (approximative value based on a 168MHz core clock)
   */
 void utils_DelayUs(uint32_t duration)
 {
 	uint32_t i 	= 0;
 	uint32_t j 	= 0;
     
 	for(i=0; i<=duration; i++)
     {
 		for(j=0; j<=26; j++)
         {
 			__asm{NOP};
 			__asm{NOP};
 		}
 	}
 }
 
 
 /**
   * @brief  "Busy wait"  delay function 
   * @param  duration: Delay time in [ms] (approximative value based on a 168MHz core clock).
   * @note This delay is approximative, and may last longer if there are many interrupts.
   */
 void utils_DelayMs(uint32_t duration)
 {
 	uint32_t i 	= 0;
 	uint32_t j 	= 0;
 	for(i=0; i<=duration; i++)
     {
 		for(j=0; j<=33600; j++)
         {
 			__asm{NOP};
 		}
 	}
 }
 
 
 /**
   * @brief  Saturate a float number between two bounds.
   * @param  val: value to constrain between two limits.
   * @param	min: minimum
   * @param	max: maximum
   * @retval None.
   */
 void utils_SaturateF(float32_t *val, float32_t min, float32_t max)
 {
 	if(*val < min)
 		*val = min;
 	else if(*val > max)
 		*val = max;
 }
 
 /**
-  * @brief  Saturate a float number between two bounds.
+  * @brief  Saturate an integer number between two bounds.
   * @param  val: value to constrain between the two limits.
   * @param	min: lower limit.
   * @param	max: upper limit.
   * @retval None.
   */
 void utils_SaturateU(uint32_t *val, uint32_t min, uint32_t max)
 {
 	if(*val < min)
 		*val = min;
 	else if(*val > max)
 		*val = max;
 }
 
 /**
   * @brief  Compute the mean of the array values.
   * @param  array: array of float number to get the mean from.
   * @param	size: size of the array.
   * @retval the mean of the array values.
   */
 float32_t utils_Mean(float32_t *array, int size)
 {
     int i;
     float32_t sum = 0.0f;
     
     for(i=0; i<size; i++)
         sum += array[i];
     
     return sum / (float32_t) size;
 }