diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index b25311a..faa25fc 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,6 +1,6 @@
 variables:
   SW_MAJOR: "0"
-  SW_MINOR: "1"
+  SW_MINOR: "2"
   APP: "engine_control"
 
 before_script:
diff --git a/source/application/collision_ctrl.c b/source/application/collision_ctrl.c
new file mode 100644
index 0000000..4948175
--- /dev/null
+++ b/source/application/collision_ctrl.c
@@ -0,0 +1,71 @@
+/*
+ * collision_ctrl.c
+ *
+ *  Created on: Aug 30, 2016
+ *      Author: tkl
+ */
+
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "board_devices.h"
+#include "queue.h"
+#include "list.h"
+#include "stack.h"
+#include "kernel.h"
+#include "driver.h"
+#include "shell.h"
+#include "drive_ctrl.h"
+#include "engine_ctrl.h"
+
+#pragma pack(push)
+#pragma pack(1)
+struct collision_ctrl_object {
+	int distance_mm;
+};
+#pragma pack(pop)
+
+struct collision_ctrl_object collision_ctrl_object;
+struct thread_context collision_ctrl_ctx;
+stack_t collision_ctrl_stack[256];
+
+static void collision_ctrl_loop(void *arg)
+{
+	drv_open(&pwm5_c2);	/* trigger pwm */
+	drv_open(&pwm2_c4); /* read pwm */
+	drv_open(&rng);
+	while(1) {
+		int pulse_ns = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
+		collision_ctrl_object.distance_mm = 343 * pulse_ns / 1000 / 2;
+		sleep_ms(1);
+	}
+}
+
+int collision_ctrl_init(void)
+{
+	thread_create(
+			&collision_ctrl_ctx,
+			collision_ctrl_stack,
+			sizeof(collision_ctrl_stack) / sizeof(stack_t),
+			collision_ctrl_loop,
+			NULL,
+			THREAD_PRIO_LOW);
+
+	return 0;
+}
+
+int collision_ctrl_get_distance_mm(void)
+{
+	return collision_ctrl_object.distance_mm;
+}
+
+bool collision_ctrl_is_collision(void)
+{
+	if(collision_ctrl_object.distance_mm < 400)
+		return true;
+	return false;
+}
diff --git a/source/application/drive_ctrl.c b/source/application/drive_ctrl.c
index c232a1d..2bf6a34 100644
--- a/source/application/drive_ctrl.c
+++ b/source/application/drive_ctrl.c
@@ -5,121 +5,146 @@
  *      Author: tkl
  */
 
+#include <stddef.h>
 #include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
 
+#include "queue.h"
+#include "stack.h"
+#include "kernel.h"
 #include "driver.h"
-#include "board_devices.h"
-#include "engines.h"
+#include "engine_ctrl.h"
 
 #include "drive_ctrl.h"
 
-struct drive_ctrl {
-	struct engine_ctrl *left_forward;
-	struct engine_ctrl *left_backward;
-	struct engine_ctrl *right_forward;
-	struct engine_ctrl *right_backward;
-};
-
-struct drive_ctrl_object {
-	unsigned int current_speed;
-};
-
-static struct drive_ctrl drive_ctrl = {
-		.left_forward = &left_forward,
-		.left_backward = &left_backward,
-		.right_forward = &right_forward,
-		.right_backward = &right_backward,
-};
-
-static struct drive_ctrl_object drive_ctrl_object = {
-		.current_speed = 50, /* % */
-};
-
 int drive_ctrl_init(void)
 {
-	unsigned int duty = 0;
+	return engine_ctrl_init();
+}
 
-	/* open enable pins */
-	drv_open(drive_ctrl.left_forward->enable);
-	drv_write(drive_ctrl.left_forward->enable, "0", 1);
-	drv_open(drive_ctrl.left_backward->enable);
-	drv_write(drive_ctrl.left_backward->enable, "0", 1);
-	drv_open(drive_ctrl.right_forward->enable);
-	drv_write(drive_ctrl.right_forward->enable, "0", 1);
-	drv_open(drive_ctrl.right_backward->enable);
-	drv_write(drive_ctrl.right_backward->enable, "0", 1);
-
-	/* open pwm's */
-	drv_open(drive_ctrl.left_backward->pwm);
-	drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-	drv_open(drive_ctrl.left_forward->pwm);
-	drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-	drv_open(drive_ctrl.right_backward->pwm);
-	drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-	drv_open(drive_ctrl.right_forward->pwm);
-	drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-
-	/* enable enable pins */
-	drv_write(drive_ctrl.left_forward->enable, "1", 1);
-	drv_write(drive_ctrl.left_backward->enable, "1", 1);
-	drv_write(drive_ctrl.right_forward->enable, "1", 1);
-	drv_write(drive_ctrl.right_backward->enable, "1", 1);
+int drive_ctrl_turn_right(void)
+{
+	drive_ctrl_halt();
+	int turn_speed = engine_ctrl_get_turn_speed_value();
+	for(int i = 0; i < turn_speed; i++) {
+		engine_ctrl_set_speed_left(i);
+		engine_ctrl_set_speed_right(-1 * i);
+		sleep_ms(10);
+	}
 
 	return 0;
 }
 
-static unsigned int calc_speed_front(int speed_percent)
+int drive_ctrl_turn_left(void)
 {
-	unsigned int speed_front = 0;
-	if(speed_percent > 0) {
-		if(speed_percent > 100)
-			speed_front = 100;
-		else
-			speed_front = (unsigned int) speed_percent;
+	drive_ctrl_halt();
+	int turn_speed = engine_ctrl_get_turn_speed_value();
+	for(int i = 0; i < turn_speed; i++) {
+		engine_ctrl_set_speed_right(i);
+		engine_ctrl_set_speed_left(-1 * i);
+		sleep_ms(10);
 	}
-	return speed_front;
-}
-
-static unsigned int calc_speed_back(int speed_percent)
-{
-	unsigned int speed_back = 0;
-	if(speed_percent < 0) {
-		if(speed_percent < -100)
-			speed_back = 100;
-		else
-			speed_back = (unsigned int) abs(speed_percent);
-	}
-	return speed_back;
-}
-
-int drive_ctrl_set_speed_left(int speed_percent)
-{
-	unsigned int front_speed = calc_speed_front(speed_percent);
-	unsigned int back_speed = calc_speed_back(speed_percent);
-	int ret = drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
-	ret |= drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed);
-
-	return ret;
-}
-
-int drive_ctrl_set_speed_right(int speed_percent)
-{
-	unsigned int front_speed = calc_speed_front(speed_percent);
-	unsigned int back_speed = calc_speed_back(speed_percent);
-	int ret = drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
-	ret |= drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed);
-
-	return ret;
-}
-
-int drive_ctrl_set_speed_value(int speed_percent)
-{
-	drive_ctrl_object.current_speed = speed_percent;
 
 	return 0;
 }
 
-int drive_ctrl_get_speed_value(void)
+int drive_ctrl_boost(void)
 {
-	return drive_ctrl_object.current_speed;
+	int speed = engine_ctrl_get_target_speed_value();
+	speed += 10;
+	engine_ctrl_set_target_speed_value(speed);
+	engine_ctrl_set_speed_left(speed);
+	engine_ctrl_set_speed_right(speed);
+
+	return 0;
+}
+
+int drive_ctrl_retard(void)
+{
+	int speed = engine_ctrl_get_target_speed_value();
+	speed -= 10;
+	engine_ctrl_set_target_speed_value(speed);
+	engine_ctrl_set_speed_left(speed);
+	engine_ctrl_set_speed_right(speed);
+
+	return 0;
+}
+
+int drive_ctrl_halt(void)
+{
+	int current_speed = engine_ctrl_get_current_speed_value();
+	if(current_speed > 0) {
+		for(int i = current_speed; i > 0; i--) {
+			engine_ctrl_set_current_speed_value(i);
+			engine_ctrl_set_speed_left(i);
+			engine_ctrl_set_speed_right(i);
+			sleep_ms(10);
+		}
+	}
+	else {
+		for(int i = current_speed; i < 0; i++) {
+			engine_ctrl_set_current_speed_value(i);
+			engine_ctrl_set_speed_left(i);
+			engine_ctrl_set_speed_right(i);
+			sleep_ms(10);
+		}
+	}
+
+	return 0;
+}
+
+int drive_ctrl_forward(void)
+{
+	int target_speed = engine_ctrl_get_target_speed_value();
+	int current_speed = engine_ctrl_get_current_speed_value();
+	if(target_speed < 0)
+		target_speed = abs(target_speed);
+	engine_ctrl_set_target_speed_value(target_speed);
+	if(current_speed < target_speed) {
+		for(int i = current_speed; i < target_speed; i++) {
+			engine_ctrl_set_current_speed_value(i);
+			engine_ctrl_set_speed_left(i);
+			engine_ctrl_set_speed_right(i);
+			sleep_ms(10);
+		}
+	}
+	else {
+		for(int i = current_speed; i > target_speed; i--) {
+			engine_ctrl_set_current_speed_value(i);
+			engine_ctrl_set_speed_left(i);
+			engine_ctrl_set_speed_right(i);
+			sleep_ms(10);
+		}
+	}
+
+	return 0;
+}
+
+int drive_ctrl_backward(void)
+{
+	int target_speed = engine_ctrl_get_target_speed_value();
+	int current_speed = engine_ctrl_get_current_speed_value();
+	if(target_speed > 0)
+		target_speed *= -1;
+	engine_ctrl_set_target_speed_value(target_speed);
+	if(current_speed > target_speed) {
+		for(int i = current_speed; i > target_speed; i--) {
+			engine_ctrl_set_current_speed_value(i);
+			engine_ctrl_set_speed_left(i);
+			engine_ctrl_set_speed_right(i);
+			sleep_ms(10);
+		}
+	}
+	else {
+		for(int i = current_speed; i < target_speed; i++) {
+			engine_ctrl_set_current_speed_value(i);
+			engine_ctrl_set_speed_left(i);
+			engine_ctrl_set_speed_right(i);
+			sleep_ms(10);
+		}
+	}
+
+	return 0;
 }
diff --git a/source/application/engine_ctrl.c b/source/application/engine_ctrl.c
new file mode 100644
index 0000000..bb2f3c7
--- /dev/null
+++ b/source/application/engine_ctrl.c
@@ -0,0 +1,134 @@
+/*
+ * engine_ctrl.c
+ *
+ *  Created on: Aug 30, 2016
+ *      Author: tkl
+ */
+#include <stdlib.h>
+
+#include "board_devices.h"
+#include "driver.h"
+#include "engines.h"
+
+struct drive_ctrl {
+	struct engine_ctrl *left_forward;
+	struct engine_ctrl *left_backward;
+	struct engine_ctrl *right_forward;
+	struct engine_ctrl *right_backward;
+};
+
+struct drive_ctrl_object {
+	unsigned int target_speed;
+	unsigned int turn_speed;
+	unsigned int current_speed;
+};
+
+static struct drive_ctrl drive_ctrl = {
+		.left_forward = &left_forward,
+		.left_backward = &left_backward,
+		.right_forward = &right_forward,
+		.right_backward = &right_backward,
+};
+
+static struct drive_ctrl_object drive_ctrl_object = {
+		.target_speed = 100,/* % */
+		.turn_speed = 40,	/* % */
+		.current_speed = 0,	/* % */
+};
+
+int engine_ctrl_init(void)
+{
+	unsigned int duty = 0;
+
+	/* open pwm's */
+	drv_open(drive_ctrl.left_backward->pwm);
+	drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+	drv_open(drive_ctrl.left_forward->pwm);
+	drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+	drv_open(drive_ctrl.right_backward->pwm);
+	drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+	drv_open(drive_ctrl.right_forward->pwm);
+	drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+
+	return 0;
+}
+
+static unsigned int calc_speed_front(int speed_percent)
+{
+	unsigned int speed_front = 0;
+	if(speed_percent > 0) {
+		if(speed_percent > 100)
+			speed_front = 100;
+		else
+			speed_front = (unsigned int) speed_percent;
+	}
+	return speed_front;
+}
+
+static unsigned int calc_speed_back(int speed_percent)
+{
+	unsigned int speed_back = 0;
+	if(speed_percent < 0) {
+		if(speed_percent < -100)
+			speed_back = 100;
+		else
+			speed_back = (unsigned int) abs(speed_percent);
+	}
+	return speed_back;
+}
+
+int engine_ctrl_set_speed_left(int speed_percent)
+{
+	unsigned int front_speed = calc_speed_front(speed_percent);
+	unsigned int back_speed = calc_speed_back(speed_percent);
+	int ret = drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed);
+	ret |= drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
+
+	return ret;
+}
+
+int engine_ctrl_set_speed_right(int speed_percent)
+{
+	unsigned int front_speed = calc_speed_front(speed_percent);
+	unsigned int back_speed = calc_speed_back(speed_percent);
+	int ret = drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed);
+	ret |= drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
+
+	return ret;
+}
+
+int engine_ctrl_set_target_speed_value(int speed_percent)
+{
+	drive_ctrl_object.target_speed = speed_percent;
+
+	return 0;
+}
+
+int engine_ctrl_get_target_speed_value(void)
+{
+	return drive_ctrl_object.target_speed;
+}
+
+int engine_ctrl_set_current_speed_value(int speed_percent)
+{
+	drive_ctrl_object.current_speed = speed_percent;
+
+	return 0;
+}
+
+int engine_ctrl_get_current_speed_value(void)
+{
+	return drive_ctrl_object.current_speed;
+}
+
+int drive_ctrl_set_turn_speed_value(int speed_percent)
+{
+	drive_ctrl_object.turn_speed = speed_percent;
+
+	return 0;
+}
+
+int engine_ctrl_get_turn_speed_value(void)
+{
+	return drive_ctrl_object.turn_speed;
+}
diff --git a/source/application/include/collision_ctrl.h b/source/application/include/collision_ctrl.h
new file mode 100644
index 0000000..f05f268
--- /dev/null
+++ b/source/application/include/collision_ctrl.h
@@ -0,0 +1,15 @@
+/*
+ * collision_ctrl.h
+ *
+ *  Created on: Aug 30, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_
+#define SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_
+
+int collision_ctrl_init(void);
+int collision_ctrl_get_distance_mm(void);
+bool collision_ctrl_is_collision(void);
+
+#endif /* SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_ */
diff --git a/source/application/include/drive_ctrl.h b/source/application/include/drive_ctrl.h
index f499a5b..8edab17 100644
--- a/source/application/include/drive_ctrl.h
+++ b/source/application/include/drive_ctrl.h
@@ -10,10 +10,12 @@
 
 int drive_ctrl_init(void);
 
-int drive_ctrl_set_speed_left(int speed_percent);
-int drive_ctrl_set_speed_right(int speed_percent);
-int drive_ctrl_set_speed_value(int speed_percent);
-int drive_ctrl_get_speed_value(void);
-
+int drive_ctrl_turn_right(void);
+int drive_ctrl_turn_left(void);
+int drive_ctrl_boost(void);
+int drive_ctrl_retard(void);
+int drive_ctrl_halt(void);
+int drive_ctrl_forward(void);
+int drive_ctrl_backward(void);
 
 #endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */
diff --git a/source/application/include/engine_ctrl.h b/source/application/include/engine_ctrl.h
new file mode 100644
index 0000000..245451f
--- /dev/null
+++ b/source/application/include/engine_ctrl.h
@@ -0,0 +1,22 @@
+/*
+ * engine_ctrl.h
+ *
+ *  Created on: Aug 30, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_INCLUDE_ENGINE_CTRL_H_
+#define SOURCE_APPLICATION_INCLUDE_ENGINE_CTRL_H_
+
+int engine_ctrl_init(void);
+
+int engine_ctrl_set_speed_left(int speed_percent);
+int engine_ctrl_set_speed_right(int speed_percent);
+int engine_ctrl_set_target_speed_value(int speed_percent);
+int engine_ctrl_get_target_speed_value(void);
+int engine_ctrl_set_current_speed_value(int speed_percent);
+int engine_ctrl_get_current_speed_value(void);
+int engine_ctrl_set_turn_speed_value(int speed_percent);
+int engine_ctrl_get_turn_speed_value(void);
+
+#endif /* SOURCE_APPLICATION_INCLUDE_ENGINE_CTRL_H_ */
diff --git a/source/application/include/engines.h b/source/application/include/engines.h
index 8957632..cc6d7ed 100644
--- a/source/application/include/engines.h
+++ b/source/application/include/engines.h
@@ -10,27 +10,22 @@
 
 struct engine_ctrl {
 	const struct driver *pwm;
-	const struct driver *enable;
-};
-
-static struct engine_ctrl right_forward = {
-		.pwm = &pwm_2,
-		.enable = &gpio_c0,
-};
-
-static struct engine_ctrl right_backward = {
-		.pwm = &pwm_1,
-		.enable = &gpio_c1,
 };
 
 static struct engine_ctrl left_forward = {
 		.pwm = &pwm_4,
-		.enable = &gpio_c2,
+};
+
+static struct engine_ctrl right_backward = {
+		.pwm = &pwm_3,
+};
+
+static struct engine_ctrl right_forward = {
+		.pwm = &pwm_2,
 };
 
 static struct engine_ctrl left_backward = {
-		.pwm = &pwm_3,
-		.enable = &gpio_c3,
+		.pwm = &pwm_1,
 };
 
 #endif /* SOURCE_APPLICATION_INCLUDE_ENGINES_H_ */
diff --git a/source/application/include/sensor_data.h b/source/application/include/sensor_data.h
new file mode 100644
index 0000000..ed4e245
--- /dev/null
+++ b/source/application/include/sensor_data.h
@@ -0,0 +1,14 @@
+/*
+ * sensor_data.h
+ *
+ *  Created on: Aug 30, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_INCLUDE_SENSOR_DATA_H_
+#define SOURCE_APPLICATION_INCLUDE_SENSOR_DATA_H_
+
+int sensor_data_init(void);
+int sensor_data_sys_msg(bool state);
+
+#endif /* SOURCE_APPLICATION_INCLUDE_SENSOR_DATA_H_ */
diff --git a/source/application/include/system_state.h b/source/application/include/system_state.h
new file mode 100644
index 0000000..379f624
--- /dev/null
+++ b/source/application/include/system_state.h
@@ -0,0 +1,26 @@
+/*
+ * system_state.h
+ *
+ *  Created on: Sep 19, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_INCLUDE_SYSTEM_STATE_H_
+#define SOURCE_APPLICATION_INCLUDE_SYSTEM_STATE_H_
+
+enum system_state {
+	SYS_INIT = 0,
+	SYS_IDLE,
+	SYS_DRIVING,
+	SYS_COLLISION_DETECTED,
+	SYS_COLLISION_DRIVE_BACK,
+	SYS_COLLISION_TURN,
+	SYS_MAX
+};
+
+int system_state_init(void);
+int system_state_poll(void);
+int system_state_force(enum system_state state);
+
+
+#endif /* SOURCE_APPLICATION_INCLUDE_SYSTEM_STATE_H_ */
diff --git a/source/application/main.c b/source/application/main.c
index 4544360..5204f2a 100644
--- a/source/application/main.c
+++ b/source/application/main.c
@@ -10,15 +10,22 @@
 #include "shell.h"
 #include "shell_commands.h"
 #include "drive_ctrl.h"
+#include "collision_ctrl.h"
+#include "sensor_data.h"
+#include "system_state.h"
 
 int main(void)
 {
-	drive_ctrl_init();
 	shell_init(&uart_1);
 	shell_commands_init();
+	drive_ctrl_init();
+	collision_ctrl_init();
+	sensor_data_init();
+	system_state_init();
 
 	while(1) {
-		sleep_ms(1000);
+		system_state_poll();
+		sleep_ms(0);
 	}
 
 	return 0;
diff --git a/source/application/sensor_data.c b/source/application/sensor_data.c
new file mode 100644
index 0000000..7b72a50
--- /dev/null
+++ b/source/application/sensor_data.c
@@ -0,0 +1,65 @@
+/*
+ * sensor_data.c
+ *
+ *  Created on: Aug 30, 2016
+ *      Author: tkl
+ */
+
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include "driver.h"
+#include "queue.h"
+#include "list.h"
+#include "stack.h"
+#include "kernel.h"
+#include "shell.h"
+#include "collision_ctrl.h"
+
+struct sensor_data_object {
+	bool sys_msg_on;
+};
+
+struct sensor_data_object sensor_data_object = {
+		.sys_msg_on = false,
+};
+
+struct thread_context sensor_data_ctx;
+stack_t sensor_data_stack[256];
+
+static void sensor_data_loop(void *arg)
+{
+	char print_buffer[50];
+	while(1) {
+		int collision_distance = collision_ctrl_get_distance_mm();
+		if(sensor_data_object.sys_msg_on) {
+			sprintf(print_buffer, "#SYS:%d\r\n", collision_distance);
+			shell_write(print_buffer, strlen(print_buffer));
+		}
+		sleep_ms(1000);
+	}
+}
+
+int sensor_data_init(void)
+{
+	thread_create(
+			&sensor_data_ctx,
+			sensor_data_stack,
+			sizeof(sensor_data_stack) / sizeof(stack_t),
+			sensor_data_loop,
+			NULL,
+			THREAD_PRIO_LOW);
+
+	return 0;
+}
+
+int sensor_data_sys_msg(bool state)
+{
+	sensor_data_object.sys_msg_on = state;
+
+	return 0;
+}
diff --git a/source/application/shell_commands.c b/source/application/shell_commands.c
index 0cb89f8..c584da2 100644
--- a/source/application/shell_commands.c
+++ b/source/application/shell_commands.c
@@ -6,21 +6,37 @@
  */
 #include <stddef.h>
 #include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
 
 #include "list.h"
 #include "driver.h"
 #include "shell.h"
-#include "drive_ctrl.h"
+#include "queue.h"
+#include "stack.h"
+#include "kernel.h"
 #include "version.h"
+#include "sensor_data.h"
+#include "system_state.h"
 
-static void *drive_turn_right_cb(const char *param);
-static void *drive_turn_left_cb(const char *param);
-static void *drive_boost_cb(const char *param);
-static void *drive_retard_cb(const char *param);
 static void *drive_halt_cb(const char *param);
 static void *drive_forward_cb(const char *param);
-static void *drive_backward_cb(const char *param);
 static void *app_version(const char *param);
+static void *sys_msg_on_cb(const char *param);
+static void *sys_msg_off_cb(const char *param);
+
+static struct command cmd_sys_msg_on = {
+		.command = "sys on",
+		.description = "Switch system message on.",
+		.command_callback = sys_msg_on_cb,
+};
+
+static struct command cmd_sys_msg_off = {
+		.command = "sys off",
+		.description = "Switch system message off.",
+		.command_callback = sys_msg_off_cb,
+};
 
 static struct command cmd_app_version = {
 		.command = "version",
@@ -29,148 +45,62 @@ static struct command cmd_app_version = {
 };
 
 static struct command drive_forward = {
-		.command = "f",
-		.description = "Start driving forward with preset speed.",
+		.command = "d",
+		.description = "Start driving .",
 		.command_callback = drive_forward_cb,
 };
 
-static struct command drive_backward = {
-		.command = "b",
-		.description = "Start driving backward with preset speed.",
-		.command_callback = drive_backward_cb,
-};
-
 static struct command drive_halt = {
 		.command = "h",
 		.description = "Stop driving.",
 		.command_callback = drive_halt_cb,
 };
 
-static struct command drive_boost = {
-		.command = "+",
-		.description = "Boost speed.",
-		.command_callback = drive_boost_cb,
-};
-
-static struct command drive_retard = {
-		.command = "-",
-		.description = "Retard speed",
-		.command_callback = drive_retard_cb,
-};
-
-static struct command drive_turn_left = {
-		.command = "l",
-		.description = "Turn left.",
-		.command_callback = drive_turn_left_cb,
-};
-
-static struct command drive_turn_right = {
-		.command = "r",
-		.description = "Turn right",
-		.command_callback = drive_turn_right_cb,
-};
-
 int shell_commands_init(void)
 {
-	shell_add_command(&drive_forward);
-	shell_add_command(&drive_backward);
-	shell_add_command(&drive_boost);
-	shell_add_command(&drive_retard);
-	shell_add_command(&drive_halt);
-	shell_add_command(&drive_turn_left);
-	shell_add_command(&drive_turn_right);
 	shell_add_command(&cmd_app_version);
+	shell_add_command(&drive_forward);
+	shell_add_command(&drive_halt);
+	shell_add_command(&cmd_sys_msg_off);
+	shell_add_command(&cmd_sys_msg_on);
 
 	return 0;
 }
 
-static void *drive_turn_right_cb(const char *param)
-{
-	int speed = drive_ctrl_get_speed_value();
-	drive_ctrl_set_speed_left(abs(speed));
-	if(speed > 0)
-		speed *= -1;
-	drive_ctrl_set_speed_right(speed);
-
-	return NULL;
-}
-
-static void *drive_turn_left_cb(const char *param)
-{
-	int speed = drive_ctrl_get_speed_value();
-	drive_ctrl_set_speed_right(abs(speed));
-	if(speed > 0)
-		speed *= -1;
-	drive_ctrl_set_speed_left(speed);
-
-	return NULL;
-}
-
-static void *drive_boost_cb(const char *param)
-{
-	int speed = drive_ctrl_get_speed_value();
-	speed += 10;
-	drive_ctrl_set_speed_value(speed);
-	drive_ctrl_set_speed_left(speed);
-	drive_ctrl_set_speed_right(speed);
-
-	return NULL;
-}
-
-static void *drive_retard_cb(const char *param)
-{
-	int speed = drive_ctrl_get_speed_value();
-	speed -= 10;
-	drive_ctrl_set_speed_value(speed);
-	drive_ctrl_set_speed_left(speed);
-	drive_ctrl_set_speed_right(speed);
-
-	return NULL;
-}
-
 static void *drive_halt_cb(const char *param)
 {
-	drive_ctrl_set_speed_left(0);
-	drive_ctrl_set_speed_right(0);
-
+	system_state_force(SYS_IDLE);
 	return NULL;
 }
 
 static void *drive_forward_cb(const char *param)
 {
-	int speed = drive_ctrl_get_speed_value();
-	if(speed < 0)
-		speed = abs(speed);
-	drive_ctrl_set_speed_value(speed);
-	drive_ctrl_set_speed_left(speed);
-	drive_ctrl_set_speed_right(speed);
-
-	return NULL;
-}
-
-static void *drive_backward_cb(const char *param)
-{
-	int speed = drive_ctrl_get_speed_value();
-	if(speed > 0)
-		speed *= -1;
-	drive_ctrl_set_speed_value(speed);
-	drive_ctrl_set_speed_left(speed);
-	drive_ctrl_set_speed_right(speed);
-
+	system_state_force(SYS_DRIVING);
 	return NULL;
 }
 
 static void *app_version(const char *param)
 {
-#if 0
 	char *greeter = "engine_control version: ";
 
-	drv_write(shell_object.shell_device, greeter, strlen(greeter));
-	drv_write(shell_object.shell_device, MAJOR_VERSION, strlen(MAJOR_VERSION));
-	drv_write(shell_object.shell_device, ".", 1);
-	drv_write(shell_object.shell_device, MINOR_VERSION, strlen(MINOR_VERSION));
-	drv_write(shell_object.shell_device, ".", 1);
-	drv_write(shell_object.shell_device, BUILD_NUMBER, strlen(BUILD_NUMBER));
-#endif
+	shell_write(greeter, strlen(greeter));
+	shell_write(MAJOR_VERSION, strlen(MAJOR_VERSION));
+	shell_write(".", 1);
+	shell_write(MINOR_VERSION, strlen(MINOR_VERSION));
+	shell_write(".", 1);
+	shell_write(BUILD_NUMBER, strlen(BUILD_NUMBER));
+	shell_write("\r\n", 2);
+
+	return NULL;
+}
+
+static void *sys_msg_on_cb(const char *param)
+{
+	sensor_data_sys_msg(true);
+	return NULL;
+}
+static void *sys_msg_off_cb(const char *param)
+{
+	sensor_data_sys_msg(false);
 	return NULL;
 }
diff --git a/source/application/system_state.c b/source/application/system_state.c
new file mode 100644
index 0000000..c26e733
--- /dev/null
+++ b/source/application/system_state.c
@@ -0,0 +1,222 @@
+/*
+ * system_state.c
+ *
+ *  Created on: Sep 19, 2016
+ *      Author: tkl
+ */
+
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "collision_ctrl.h"
+#include "drive_ctrl.h"
+
+#include "driver.h"
+#include "stack.h"
+#include "queue.h"
+#include "list.h"
+#include "shell.h"
+#include "kernel.h"
+
+#include "board_devices.h"
+
+#include "system_state.h"
+
+#pragma pack(push)
+#pragma pack(1)
+struct collision_timer {
+	bool running;
+	unsigned long tick;
+};
+#pragma pack(pop)
+
+struct system_state_object {
+	enum system_state system_state;
+	enum system_state last_state;
+	enum system_state saved_state;
+	enum system_state force_state;
+	struct collision_timer collision_timer;
+};
+
+static struct system_state_object system_state_object = {
+		.system_state = SYS_INIT,
+		.last_state = SYS_MAX,
+		.saved_state = SYS_MAX,
+		.force_state = SYS_MAX,
+		.collision_timer.running = false,
+		.collision_timer.tick = 0,
+};
+
+int system_state_init(void)
+{
+	return 0;
+}
+
+static bool is_transition(void)
+{
+	return (system_state_object.system_state != system_state_object.last_state) ? true : false;
+}
+
+static enum system_state handle_state_init(void)
+{
+	enum system_state next_state = SYS_INIT;
+	if(is_transition()) {
+		system_state_object.last_state = system_state_object.system_state;
+		char print_buffer[] = "#SYS_ST: INIT\r\n";
+		shell_write(print_buffer, strlen(print_buffer));
+		drive_ctrl_halt();
+	}
+	// TODO: check various sensor states e.g. battery, position state etc...
+	next_state = SYS_IDLE;
+	return next_state;
+}
+
+static enum system_state handle_state_idle(void)
+{
+	enum system_state next_state = SYS_IDLE;
+	if(is_transition()) {
+		system_state_object.last_state = system_state_object.system_state;
+		char print_buffer[] = "#SYS_ST: IDLE\r\n";
+		shell_write(print_buffer, strlen(print_buffer));
+		drive_ctrl_halt();
+	}
+	if(system_state_object.force_state == SYS_DRIVING) {
+		next_state = SYS_DRIVING;
+		system_state_object.force_state = SYS_MAX;
+	}
+	return next_state;
+}
+
+static enum system_state handle_state_driving(void)
+{
+	enum system_state next_state = SYS_DRIVING;
+	if(is_transition()) {
+		system_state_object.last_state = system_state_object.system_state;
+		char print_buffer[] = "#SYS_ST: DRIVING\r\n";
+		shell_write(print_buffer, strlen(print_buffer));
+		drive_ctrl_forward();
+	}
+	if(collision_ctrl_is_collision()) {
+		next_state = SYS_COLLISION_DETECTED;
+		system_state_object.saved_state = system_state_object.system_state;
+	}
+	if(system_state_object.force_state == SYS_IDLE) {
+		next_state = SYS_IDLE;
+		system_state_object.force_state = SYS_MAX;
+	}
+
+	return next_state;
+}
+
+static enum system_state handle_state_collision_detected(void)
+{
+	enum system_state next_state = SYS_COLLISION_DETECTED;
+	if(is_transition()) {
+		system_state_object.last_state = system_state_object.system_state;
+		drive_ctrl_halt();
+		char print_buffer[] = "#SYS_ST: COLLISION_DETECTED\r\n";
+		shell_write(print_buffer, strlen(print_buffer));
+	}
+	next_state = SYS_COLLISION_DRIVE_BACK;
+	if(system_state_object.force_state == SYS_IDLE) {
+		next_state = SYS_IDLE;
+		system_state_object.force_state = SYS_MAX;
+	}
+	return next_state;
+}
+
+static enum system_state handle_state_collision_drive_back(void)
+{
+	enum system_state next_state = SYS_COLLISION_DRIVE_BACK;
+	if(is_transition()) {
+		system_state_object.last_state = system_state_object.system_state;
+		char print_buffer[] = "#SYS_ST: COLLISION_DRIVE_BACK\r\n";
+		shell_write(print_buffer, strlen(print_buffer));
+		system_state_object.collision_timer.tick = sys_tick_get_ms() + 500;
+		system_state_object.collision_timer.running = true;
+	}
+	if(system_state_object.collision_timer.running &&
+			(system_state_object.collision_timer.tick < sys_tick_get_ms())) {
+		drive_ctrl_halt();
+		next_state = SYS_COLLISION_TURN;
+		system_state_object.collision_timer.running = false;
+	}
+	if(system_state_object.force_state == SYS_IDLE) {
+		next_state = SYS_IDLE;
+		system_state_object.force_state = SYS_MAX;
+		system_state_object.collision_timer.running = false;
+	}
+	return next_state;
+}
+
+static enum system_state handle_state_collision_turn(void)
+{
+	enum system_state next_state = SYS_COLLISION_TURN;
+	if(is_transition()) {
+		system_state_object.last_state = system_state_object.system_state;
+		char print_buffer[] = "#SYS_ST: COLLISION_TURN\r\n";
+		shell_write(print_buffer, strlen(print_buffer));
+		drv_open(&rng);
+		// random number between 500 and 4000
+		unsigned int sleep = (unsigned int)drv_read(&rng, NULL, 0) % 3500 + 500;
+		drv_close(&rng);
+		system_state_object.collision_timer.tick = sys_tick_get_ms() + sleep;
+		system_state_object.collision_timer.running = true;
+		// randomize turn direction
+		if(sleep % 2)
+			drive_ctrl_turn_left();
+		else
+			drive_ctrl_turn_right();
+	}
+	if(system_state_object.collision_timer.running &&
+			(system_state_object.collision_timer.tick < sys_tick_get_ms())) {
+		drive_ctrl_halt();
+		next_state = system_state_object.saved_state;
+		system_state_object.collision_timer.running = false;
+	}
+	if(system_state_object.force_state == SYS_IDLE) {
+		next_state = SYS_IDLE;
+		system_state_object.force_state = SYS_MAX;
+		system_state_object.collision_timer.running = false;
+	}
+	return next_state;
+}
+
+int system_state_poll(void)
+{
+	enum system_state next_state = SYS_MAX;
+	switch(system_state_object.system_state) {
+	case SYS_INIT:
+		next_state = handle_state_init();
+		break;
+	case SYS_IDLE:
+		next_state = handle_state_idle();
+		break;
+	case SYS_DRIVING:
+		next_state = handle_state_driving();
+		break;
+	case SYS_COLLISION_DETECTED:
+		next_state = handle_state_collision_detected();
+		break;
+	case SYS_COLLISION_DRIVE_BACK:
+		next_state = handle_state_collision_drive_back();
+		break;
+	case SYS_COLLISION_TURN:
+		next_state = handle_state_collision_turn();
+		break;
+	case SYS_MAX:
+		break;
+	}
+	if(next_state < SYS_MAX) {
+		system_state_object.system_state = next_state;
+		return 0;
+	}
+	return -1;
+}
+
+int system_state_force(enum system_state state)
+{
+	system_state_object.force_state = state;
+	return 0;
+}
diff --git a/source/os/release/include/board_devices.h b/source/os/release/include/board_devices.h
index ec42636..129e41c 100644
--- a/source/os/release/include/board_devices.h
+++ b/source/os/release/include/board_devices.h
@@ -3,6 +3,7 @@
 #ifndef BOARD_DEVICES_H
 #define BOARD_DEVICES_H
 
+extern const struct driver rng;
 extern const struct driver gpio_c0;
 extern const struct driver gpio_c1;
 extern const struct driver gpio_c2;
@@ -11,6 +12,8 @@ extern const struct driver pwm_4;
 extern const struct driver pwm_3;
 extern const struct driver pwm_2;
 extern const struct driver pwm_1;
+extern const struct driver pwm5_c2;
+extern const struct driver pwm2_c4;
 extern const struct driver uart_1;
 
 #endif /* BOARD_DEVICES_H */
diff --git a/source/os/release/include/driver.h b/source/os/release/include/driver.h
index e91349d..8af40b4 100644
--- a/source/os/release/include/driver.h
+++ b/source/os/release/include/driver.h
@@ -8,13 +8,16 @@
 #ifndef SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
 #define SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
 
-#define IOCTL_PWM_SET_DUTY_CYCLE	0
+#define IOCTL_PWM_SET_DUTY_CYCLE		0
+#define IOCTL_PWM_GET_PERIOD_NS			1
+#define IOCTL_PWM_GET_PULSE_WIDTH_NS	2
 
 enum driver_type {
 	DRIVER_TYPE_ADC,
 	DRIVER_TYPE_GPIO,
 	DRIVER_TYPE_I2C,
 	DRIVER_TYPE_PWM,
+	DRIVER_TYPE_RNG,
 	DRIVER_TYPE_RTC,
 	DRIVER_TYPE_SPI,
 	DRIVER_TYPE_UART
diff --git a/source/os/release/include/kernel.h b/source/os/release/include/kernel.h
index e69f607..ec33802 100644
--- a/source/os/release/include/kernel.h
+++ b/source/os/release/include/kernel.h
@@ -60,5 +60,6 @@ struct thread_context *thread_create(
 
 void thread_exit(void);
 void sleep_ms(unsigned int ms);
+unsigned long sys_tick_get_ms(void);
 
 #endif /* SOURCE_FIRMWARE_KERNEL_INTERFACE_KERNEL_H_ */
diff --git a/source/os/release/include/shell.h b/source/os/release/include/shell.h
index c242e8d..1b176ee 100644
--- a/source/os/release/include/shell.h
+++ b/source/os/release/include/shell.h
@@ -19,5 +19,6 @@ struct command {
 
 int shell_init(const struct driver *shell_device);
 int shell_add_command(struct command *command);
+int shell_write(const char *buffer, unsigned int len);
 
 #endif /* SOURCE_FIRMWARE_KERNEL_INTERFACE_SHELL_H_ */
diff --git a/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.4.a b/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a
similarity index 87%
rename from source/os/release/libkosmos-arm-stm32f4-discovery-0.1.4.a
rename to source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a
index ec2ac10..5fc8b54 100644
Binary files a/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.4.a and b/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a differ
diff --git a/source/os/release/libkosmos-arm-stm32f4-discovery.a b/source/os/release/libkosmos-arm-stm32f4-discovery.a
index b1368da..489e625 120000
--- a/source/os/release/libkosmos-arm-stm32f4-discovery.a
+++ b/source/os/release/libkosmos-arm-stm32f4-discovery.a
@@ -1 +1 @@
-libkosmos-arm-stm32f4-discovery-0.1.4.a
\ No newline at end of file
+libkosmos-arm-stm32f4-discovery-0.1.10.287.a
\ No newline at end of file