diff --git a/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h b/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
index d70a788..ea87330 100755
--- a/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
+++ b/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
@@ -62,7 +62,7 @@ static const TIM_OCInitTypeDef t4_output_compare_cfg = {
 		.TIM_Pulse = 0,
 };
 
-static const GPIO_InitTypeDef port_cfg = {
+static const GPIO_InitTypeDef port_cfg_D15 = {
 		.GPIO_Pin = GPIO_Pin_15,
 		.GPIO_Mode = GPIO_Mode_AF,
 		.GPIO_OType = GPIO_OType_PP,
@@ -70,18 +70,18 @@ static const GPIO_InitTypeDef port_cfg = {
 		.GPIO_Speed = GPIO_Speed_100MHz,
 };
 
-static struct stm32f4_pwm str32f4_pwm = {
+static struct stm32f4_pwm str32f4_pwm_4 = {
 		.timer = TIM4,
 		.timer_cfg = &timer_4_cfg,
 		.output_compare_cfg = &t4_output_compare_cfg,
 		.port = GPIOD,
 		.pin_src = GPIO_PinSource15,
-		.port_cfg = &port_cfg,
+		.port_cfg = &port_cfg_D15,
 		.channel = channel_4,
 };
 
 static const struct pwm pwm_ch4 = {
-		.arch_dep_device = &str32f4_pwm,
+		.arch_dep_device = &str32f4_pwm_4,
 		.fp = &stm32f4_pwm_fp,
 };
 
@@ -94,6 +94,105 @@ const struct driver pwm_4 = {
 		&pwm_ch4,
 };
 
+// PWM Channel 3
+static const GPIO_InitTypeDef port_cfg_D14 = {
+		.GPIO_Pin = GPIO_Pin_14,
+		.GPIO_Mode = GPIO_Mode_AF,
+		.GPIO_OType = GPIO_OType_PP,
+		.GPIO_PuPd = GPIO_PuPd_UP,
+		.GPIO_Speed = GPIO_Speed_100MHz,
+};
+
+static struct stm32f4_pwm str32f4_pwm_3 = {
+		.timer = TIM4,
+		.timer_cfg = &timer_4_cfg,
+		.output_compare_cfg = &t4_output_compare_cfg,
+		.port = GPIOD,
+		.pin_src = GPIO_PinSource14,
+		.port_cfg = &port_cfg_D14,
+		.channel = channel_3,
+};
+
+static const struct pwm pwm_ch3 = {
+		.arch_dep_device = &str32f4_pwm_3,
+		.fp = &stm32f4_pwm_fp,
+};
+
+#ifdef TEST_APP
+static const struct driver pwm_3 = {
+#else
+const struct driver pwm_3 = {
+#endif
+		DRIVER_TYPE_PWM,
+		&pwm_ch3,
+};
+
+// PWM Channel 2
+static const GPIO_InitTypeDef port_cfg_D13 = {
+		.GPIO_Pin = GPIO_Pin_13,
+		.GPIO_Mode = GPIO_Mode_AF,
+		.GPIO_OType = GPIO_OType_PP,
+		.GPIO_PuPd = GPIO_PuPd_UP,
+		.GPIO_Speed = GPIO_Speed_100MHz,
+};
+
+static struct stm32f4_pwm str32f4_pwm_2 = {
+		.timer = TIM4,
+		.timer_cfg = &timer_4_cfg,
+		.output_compare_cfg = &t4_output_compare_cfg,
+		.port = GPIOD,
+		.pin_src = GPIO_PinSource13,
+		.port_cfg = &port_cfg_D13,
+		.channel = channel_2,
+};
+
+static const struct pwm pwm_ch2 = {
+		.arch_dep_device = &str32f4_pwm_2,
+		.fp = &stm32f4_pwm_fp,
+};
+
+#ifdef TEST_APP
+static const struct driver pwm_2 = {
+#else
+const struct driver pwm_2 = {
+#endif
+		DRIVER_TYPE_PWM,
+		&pwm_ch2,
+};
+
+// PWM Channel 2
+static const GPIO_InitTypeDef port_cfg_D12 = {
+		.GPIO_Pin = GPIO_Pin_12,
+		.GPIO_Mode = GPIO_Mode_AF,
+		.GPIO_OType = GPIO_OType_PP,
+		.GPIO_PuPd = GPIO_PuPd_UP,
+		.GPIO_Speed = GPIO_Speed_100MHz,
+};
+
+static struct stm32f4_pwm str32f4_pwm_1 = {
+		.timer = TIM4,
+		.timer_cfg = &timer_4_cfg,
+		.output_compare_cfg = &t4_output_compare_cfg,
+		.port = GPIOD,
+		.pin_src = GPIO_PinSource12,
+		.port_cfg = &port_cfg_D12,
+		.channel = channel_1,
+};
+
+static const struct pwm pwm_ch1 = {
+		.arch_dep_device = &str32f4_pwm_1,
+		.fp = &stm32f4_pwm_fp,
+};
+
+#ifdef TEST_APP
+static const struct driver pwm_1 = {
+#else
+const struct driver pwm_1 = {
+#endif
+		DRIVER_TYPE_PWM,
+		&pwm_ch3,
+};
+
 // UART 1
 static char console_linear_buffer[80];
 static struct ringbuffer console_buffer = {
@@ -154,171 +253,134 @@ const struct driver uart_1 = {
 		&__uart_1,
 };
 
-// LED 3
-static const GPIO_InitTypeDef stm32_f4_discovery_led_3_gpio = {
-		GPIO_Pin_12,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_discovery_led_3 = {
-		GPIOD,
-		&stm32_f4_discovery_led_3_gpio,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __led_3 = {
-		(void*)&stm32_f4_discovery_led_3,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver led_3 = {
-#else
-const struct driver led_3 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__led_3,
-};
-
-// LED 4
-static const GPIO_InitTypeDef stm32_f4_discovery_led_4_gpio = {
-		GPIO_Pin_13,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_discovery_led_4 = {
-		GPIOD,
-		&stm32_f4_discovery_led_4_gpio,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __led_4 = {
-		(void*)&stm32_f4_discovery_led_4,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver led_4 = {
-#else
-const struct driver led_4 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__led_4,
-};
-
-// LED 5
-static const GPIO_InitTypeDef stm32_f4_discovery_led_5_gpio = {
-		GPIO_Pin_14,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_discovery_led_5 = {
-		GPIOD,
-		&stm32_f4_discovery_led_5_gpio,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __led_5 = {
-		(void*)&stm32_f4_discovery_led_5,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver led_5 = {
-#else
-const struct driver led_5 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__led_5,
-};
-
-// LED 6
-static const GPIO_InitTypeDef stm32_f4_discovery_led_6_gpio = {
-		GPIO_Pin_15,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_discovery_led_6 = {
-		GPIOD,
-		&stm32_f4_discovery_led_6_gpio,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __led_6 = {
-		(void*)&stm32_f4_discovery_led_6,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver led_6 = {
-#else
-const struct driver led_6 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__led_6,
-};
-
-// BUTTON
-static const GPIO_InitTypeDef stm32_f4_discovery_user_button_gpio = {
+// GPIOC0
+static const GPIO_InitTypeDef port_cfg_C0 = {
 		GPIO_Pin_0,
-		GPIO_Mode_IN,
+		GPIO_Mode_OUT,
 		GPIO_Speed_100MHz,
 		GPIO_OType_PP,
 		GPIO_PuPd_NOPULL
 };
 
-static const EXTI_InitTypeDef stm32_f4_discovery_user_button_exti = {
-		EXTI_Line0,
-		EXTI_Mode_Interrupt,
-		EXTI_Trigger_Rising_Falling,
-		ENABLE
-};
-
-static const NVIC_InitTypeDef stm32_f4_discovery_user_button_nvic = {
-		EXTI0_IRQn,
-		0x0F,
-		0x0F,
-		ENABLE
-};
-
-static const struct stm32f4_gpio stm32_f4_discovery_user_button = {
-		GPIOA,
-		&stm32_f4_discovery_user_button_gpio,
-		&stm32_f4_discovery_user_button_exti,
-		&stm32_f4_discovery_user_button_nvic,
+static const struct stm32f4_gpio stm32_f4_gpio_C0 = {
+		GPIOC,
+		&port_cfg_C0,
+		NULL,
+		NULL,
 		NULL,
 		NULL
 };
 
-static const struct gpio user_button = {
-		(void*)&stm32_f4_discovery_user_button,
+static const struct gpio __gpio_c0 = {
+		(void*)&stm32_f4_gpio_C0,
 		&gpio_fp
 };
 
+#ifdef TEST_APP
+static const struct driver gpio_c0 = {
+#else
+const struct driver gpio_c0 = {
+#endif
+		DRIVER_TYPE_GPIO,
+		&__gpio_c0,
+};
+
+// GPIO_C1
+static const GPIO_InitTypeDef port_cfg_C1 = {
+		GPIO_Pin_1,
+		GPIO_Mode_OUT,
+		GPIO_Speed_100MHz,
+		GPIO_OType_PP,
+		GPIO_PuPd_NOPULL
+};
+
+static const struct stm32f4_gpio stm32_f4_gpio_C1 = {
+		GPIOC,
+		&port_cfg_C1,
+		NULL,
+		NULL,
+		NULL,
+		NULL
+};
+
+static const struct gpio __gpio_c1 = {
+		(void*)&stm32_f4_gpio_C1,
+		&gpio_fp
+};
+
+#ifdef TEST_APP
+static const struct driver gpio_c1 = {
+#else
+const struct driver gpio_c1 = {
+#endif
+		DRIVER_TYPE_GPIO,
+		&__gpio_c1,
+};
+
+// GPIO_C2
+static const GPIO_InitTypeDef port_cfg_C2 = {
+		GPIO_Pin_2,
+		GPIO_Mode_OUT,
+		GPIO_Speed_100MHz,
+		GPIO_OType_PP,
+		GPIO_PuPd_NOPULL
+};
+
+static const struct stm32f4_gpio stm32_f4_gpio_C2 = {
+		GPIOC,
+		&port_cfg_C2,
+		NULL,
+		NULL,
+		NULL,
+		NULL
+};
+
+static const struct gpio __gpio_c2 = {
+		(void*)&stm32_f4_gpio_C2,
+		&gpio_fp
+};
+
+#ifdef TEST_APP
+static const struct driver gpio_c2 = {
+#else
+const struct driver gpio_c2 = {
+#endif
+		DRIVER_TYPE_GPIO,
+		&__gpio_c2,
+};
+
+// GPIO_C3
+static const GPIO_InitTypeDef port_cfg_C3 = {
+		GPIO_Pin_1,
+		GPIO_Mode_OUT,
+		GPIO_Speed_100MHz,
+		GPIO_OType_PP,
+		GPIO_PuPd_NOPULL
+};
+
+static const struct stm32f4_gpio stm32_f4_gpio_C3 = {
+		GPIOC,
+		&port_cfg_C3,
+		NULL,
+		NULL,
+		NULL,
+		NULL
+};
+
+static const struct gpio __gpio_c3 = {
+		(void*)&stm32_f4_gpio_C3,
+		&gpio_fp
+};
+
+#ifdef TEST_APP
+static const struct driver gpio_c3 = {
+#else
+const struct driver gpio_c3 = {
+#endif
+		DRIVER_TYPE_GPIO,
+		&__gpio_c3,
+};
+
 //! \brief Setup the hardware of the stm32f4-discovery board.
 void board_init(void);
 
diff --git a/source/test/pwm/main.c b/source/test/pwm/main.c
index e8b0418..22853aa 100644
--- a/source/test/pwm/main.c
+++ b/source/test/pwm/main.c
@@ -16,6 +16,45 @@
 #include "list.h"
 #include "shell.h"
 
+struct engine_ctrl {
+	const struct driver *pwm;
+	const struct driver *enable;
+};
+
+struct drive_ctrl {
+	struct engine_ctrl *left_forward;
+	struct engine_ctrl *left_backward;
+	struct engine_ctrl *right_forward;
+	struct engine_ctrl *right_backward;
+};
+
+
+static struct engine_ctrl right_forward = {
+		.pwm = &pwm_1,
+		.enable = &gpio_c1,
+};
+
+static struct engine_ctrl right_backward = {
+		.pwm = &pwm_2,
+		.enable = &gpio_c0,
+};
+
+static struct engine_ctrl left_forward = {
+		.pwm = &pwm_3,
+		.enable = &gpio_c3,
+};
+
+static struct engine_ctrl left_backward = {
+		.pwm = &pwm_4,
+		.enable = &gpio_c2,
+};
+
+static struct drive_ctrl drive_ctrl = {
+		.left_forward = &left_forward,
+		.left_backward = &left_backward,
+		.right_forward = &right_forward,
+		.right_backward = &right_backward,
+};
 
 #define TH_STACK_SIZE  256
 stack_t th_stack[TH_STACK_SIZE];
@@ -23,19 +62,30 @@ struct thread_context th_ctx;
 static void th_func(void *arg)
 {
 	unsigned int duty = 0;
-	open(&pwm_4);
+	open(drive_ctrl.left_forward->enable);
+	write(drive_ctrl.left_forward->enable, "0", 1);
+	open(drive_ctrl.left_forward->pwm);
+	open(drive_ctrl.left_backward->enable);
+	write(drive_ctrl.left_backward->enable, "0", 1);
+	open(drive_ctrl.left_backward->pwm);
+
+	write(drive_ctrl.left_forward->enable, "1", 1);
+	write(drive_ctrl.left_backward->enable, "1", 1);
 	while(1) {
-		while(duty < 100) {
-			ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-			pwm_set_duty_cycle(&pwm_ch4, duty);
-			sleep_ms(10);
-			duty++;
+		duty = 0;
+		ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+		for(duty = 0; duty < 100; duty++) {
+			ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+			sleep_ms(100);
 		}
-		while(duty > 0) {
-			ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-			sleep_ms(10);
-			duty--;
+		duty = 0;
+		ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+		sleep_ms(100);
+		for(duty = 0; duty < 100; duty++) {
+			ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+			sleep_ms(100);
 		}
+		sleep_ms(100);
 	}
 }