diff --git a/robot/include/servos.h b/robot/include/servos.h
index ea0c094..b54222a 100644
--- a/robot/include/servos.h
+++ b/robot/include/servos.h
@@ -7,7 +7,7 @@
 #define SERVO_L_BIT       PD6
 #define SERVO_R_BIT       PD5
 
-// Not just STOP +- 500us because of rounding errors (e.g. 19.9 will become
+// Not just MID +- 500us because of rounding errors (e.g. 19.9 will become
 // 19 and not 20 because of integer division)
 #define SERVO_MID         1381UL
 #define SERVO_MAX         1883UL
@@ -21,18 +21,17 @@
 #define SERVO_R_FORW      SERVO_MIN
 #define SERVO_R_BACKW     SERVO_MAX
 
+#define SERVO_DIR_BACKW   0
+#define SERVO_DIR_FORW    1
+
+#define SERVO_90_TURN_MS  1000
+#define SERVO_CM_PER_S    15 // Distance in cm that servos travel in 1s
+
 #define PRESCALER         1024UL
 #define PWM_PERIOD_MS     (uint16_t)((256000UL * PRESCALER) / F_CPU)
-#define US2TIMER0(us)     (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS))
+#define US2TIMER0(us)     (uint8_t)((51U * us) / (200U * PWM_PERIOD_MS))
 
 void init_servos(void);
-
-void servo_full_forw(void);
-void servo_turn_forw_r(void);
-void servo_turn_forw_l(void);
-void servo_full_backw(void);
-void servo_turn_backw_r(void);
-void servo_turn_backw_l(void);
-
+void servo_speed(uint8_t direction, uint8_t load_percent_l, uint8_t load_percent_r);
 
 #endif /* SERVOS_H */
diff --git a/robot/src/main.c b/robot/src/main.c
index 83048b7..c1c61f1 100644
--- a/robot/src/main.c
+++ b/robot/src/main.c
@@ -14,7 +14,10 @@
 #include "debug.h"
 #endif /* DEBUG */
 
-#define OBSTACLE_THRESHOLD  10
+// Distance in cm after which node should start evading the obstacle
+#define OBSTACLE_THRESHOLD  20
+// Wait 2s before starting to drive
+#define START_DELAY_MS      2000
 
 uint8_t tx_raw[sizeof(data_packet_t)];
 data_packet_t tx_data;
@@ -28,31 +31,30 @@ int main(void)
         return R_PACKAGE_SIZE;
 
     init_leds();
-    init_servos();
     init_sonar();
+    init_servos();
     nrf24_init();
 
+    SET(LED_PORT, LED_BLUE_BIT);
+    _delay_ms(START_DELAY_MS);
+    CLR(LED_PORT, LED_BLUE_BIT);
+
     nrf24_config(2, sizeof(data_packet_t)); // Channel #2
     nrf24_tx_address(base_mac);
     nrf24_rx_address(node_mac);
 
-    // Use green LED as power LED
-    // TODO: need all LEDs for debugging, will use as power when UART debugger
-    // will arrive
-    /* SET(LED_PORT, LED_GREEN_BIT); */
-
     sei();
 
-    servo_full_forw();
     while (1) {
         // Distance is measured early so the same value gets sent to the base
         // station and gets used to decide movement. Needs to be done in order
         // for base station to receive meaningful distance value.
         distance_in_cm = read_sonar();
-        decide_movement(&distance_in_cm);
 
         // Update base station
         send_sensor_data(&distance_in_cm);
+
+        decide_movement(&distance_in_cm);
     }
 
     cli();
@@ -71,9 +73,9 @@ void send_sensor_data(const int32_t *distance_in_cm)
     nrf24_send(tx_raw);
 
     // Indicate package sending with a signal LED
-    SET(LED_PORT, LED_GREEN_BIT);
+    SET(LED_PORT, LED_BLUE_BIT);
     while (nrf24_isSending());
-    CLR(LED_PORT, LED_GREEN_BIT);
+    CLR(LED_PORT, LED_BLUE_BIT);
 }
 
 /// Control servos based on received distance data
@@ -82,40 +84,39 @@ void decide_movement(const int32_t *distance_in_cm)
     uint8_t avoid_obstacle = 0;
     int32_t distance_to_obstacle = *distance_in_cm;
 
-    // No obstacle found
-    if (distance_to_obstacle == ECHO_TIMEOUT) return;
-
     while (1) {
         if (distance_to_obstacle == ECHO_TIMEOUT)  {
             // No obstacle found
-            servo_full_forw();
+            servo_speed(SERVO_DIR_FORW, 100, 100);
             avoid_obstacle = 0;
         } else if (distance_to_obstacle >= 0
                    && distance_to_obstacle <= MIN_SONAR_RANGE) {
             // Obstacle is right in front, can't make a turn
-            servo_full_backw();
+            servo_speed(SERVO_DIR_BACKW, 100, 100);
+            _delay_ms(SERVO_90_TURN_MS);
             avoid_obstacle = 1;
-            _delay_ms(SONAR_DELAY);
         } else if (distance_to_obstacle > MIN_SONAR_RANGE
                    && distance_to_obstacle <= OBSTACLE_THRESHOLD) {
-            // Obstacle is close and in front, make a turn backwards
-            servo_turn_backw_r();
+            // Obstacle is close and in front, make a turn left
+            servo_speed(SERVO_DIR_FORW, 0, 100);
+            _delay_ms(SERVO_90_TURN_MS);
             avoid_obstacle = 1;
-            _delay_ms(SONAR_DELAY);
         } else {
             // TODO: factor this in decision making about maneuvers
             // Obstacle is in front, but a safe distance away
-            servo_full_forw();
+            servo_speed(SERVO_DIR_FORW, 50, 50);
             avoid_obstacle = 0;
+
         }
 
         if (avoid_obstacle == 1) {
-            SET(LED_PORT, LED_BLUE_BIT);
+            SET(LED_PORT, LED_GREEN_BIT);
         } else {
-            CLR(LED_PORT, LED_BLUE_BIT);
+            CLR(LED_PORT, LED_GREEN_BIT);
             return;
         }
 
+        _delay_ms(500);
         distance_to_obstacle = read_sonar();
     }
 }
diff --git a/robot/src/servos.c b/robot/src/servos.c
index 778b0b1..c452446 100644
--- a/robot/src/servos.c
+++ b/robot/src/servos.c
@@ -23,38 +23,65 @@ void init_servos(void)
     SET(SERVO_DDR, SERVO_R_BIT);
 }
 
-void servo_full_forw(void)
+void servo_speed(uint8_t direction, uint8_t load_percent_l, uint8_t load_percent_r)
 {
-    SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
-    SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
-}
+    // Check max and min values (0 and 100) first to avoid division since that
+    // will lose some precision
 
-void servo_turn_forw_r(void)
-{
-    SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
-    SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
-}
+    // Special case for full stop since it requires register clearing
+    if (load_percent_l == 0 && load_percent_r == 0) {
+        TCCR0A = 0x00;
+        TCCR0B = 0x00;
+        return;
+    }
 
-void servo_turn_forw_l(void)
-{
-    SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
-    SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
-}
+    // Registers have been cleared, need to set them again
+    if (TCCR0A == 0x00 || TCCR0B == 0x00)
+        init_servos();
 
-void servo_full_backw(void)
-{
-    SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
-    SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
-}
+    // Left motor
+    if (load_percent_l == 0) {
+        SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
+    } else {
+        // Forwards
+        if (direction == SERVO_DIR_FORW) {
+            if (load_percent_l == 100) {
+                SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
+            } else {
+                SERVO_L_REG = US2TIMER0(SERVO_L_STOP + ((SERVO_L_FORW - SERVO_L_STOP)
+                                                        * load_percent_l) / 100);
+            }
+        } else {
+            // Backwards
+            if (load_percent_l == 100) {
+                SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
+            } else {
+                SERVO_L_REG = US2TIMER0(SERVO_L_STOP - ((SERVO_L_STOP - SERVO_L_BACKW)
+                                                        * load_percent_l) / 100);
+            }
+        }
+    }
 
-void servo_turn_backw_r(void)
-{
-    SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
-    SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
-}
-
-void servo_turn_backw_l(void)
-{
-    SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
-    SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
+    // Right motor
+    if (load_percent_r == 0) {
+        SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
+    } else {
+        // Forwards
+        if (direction == SERVO_DIR_FORW) {
+            if (load_percent_r == 100) {
+                SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
+            } else {
+                SERVO_R_REG = US2TIMER0(SERVO_R_STOP - ((SERVO_R_STOP - SERVO_R_FORW)
+                                                        * load_percent_r) / 100);
+            }
+        } else {
+            // Backwards
+            if (load_percent_r == 100) {
+                SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
+            } else {
+                SERVO_R_REG = US2TIMER0(SERVO_R_STOP + ((SERVO_R_BACKW - SERVO_R_STOP)
+                                                        * load_percent_r) / 100);
+            }
+        }
+    }
 }