diff --git a/implementation/software/controller.c b/implementation/software/controller.c
index b2041f5..dd9b3d6 100644
--- a/implementation/software/controller.c
+++ b/implementation/software/controller.c
@@ -1,11 +1,5 @@
 #include "include/controller.h"
 
-
-void print_point(gbVec2 *point)
-{
-    printf("position x: %.4f, y: %.4f\n", point->x, point->y);
-}
-
 int controller_init(controller_t *control)
 {
     gpio_init(BTN0_PIN,BTN0_MODE);
@@ -22,6 +16,11 @@ int controller_loop(controller_t *control){
             printf("Controller Error, x and y value are out of range x: %i, y: %i\n", setpoint->point.x, setpoint->point.y);
             return CONTROLLER_ERR;
         }
+        if (setpoint->modus == PATH_MOVE) {
+            controller_raise_marker(control);
+        } else {
+            controller_lower_marker(control);
+        }
         //printf("Controller: look up x: %i, y: %i\n", setpoint->point.x, setpoint->point.y);
         int16_t angle1 = lookup_table_angle1[setpoint->point.y][setpoint->point.x];
         int16_t angle2 = lookup_table_angle2[setpoint->point.y][setpoint->point.x];
@@ -36,151 +35,6 @@ int controller_loop(controller_t *control){
     return CONTROLLER_OK;
 }
 
-/*void controller_update(event_t *event)
-{
-    controller_t *control = container_of(event, controller_t, event);
-    controller_calculate_velocity(control);
-    event_timeout_set(&(control->event_timeout), control->time_step);
-}
-
-
-int controller_setpoint(controller_t *control, gbVec2 *point)
-{
-    control->setpoint = *point;
-    controller_inverse_kinematics(control, point, &control->angle1_setpoint , &control->angle2_setpoint);
-    return 0;
-}
-
-void controller_inverse_kinematics(controller_t *control, gbVec2 *point, float *angle1, float *angle2)
-{
-    float phi = gb_arctan2(point->y, point->x);
-    float length_a2 = gb_square(control->arm_a);
-    float length_b2 = gb_square(control->arm_b);
-    float length_c2 = gb_vec2_mag2(*point);
-    float length_c = gb_sqrt(length_c2);
-    float b = gb_arccos((length_a2 + length_c2 - length_b2) / (2 * control->arm_a * length_c));
-    float c = gb_arccos((length_a2 + length_b2 - length_c2) / (2 * control->arm_a * control->arm_b));
-    *angle1 = b + phi;
-    *angle2 = *angle1 - GB_MATH_PI + c;
-}
-
-void controller_current_position(controller_t *control, gbVec2 *point)
-{
-    float angle_a;
-    stepper_get_angle(control->stepper_a, &angle_a);
-    float angle_b;
-    stepper_get_angle(control->stepper_b, &angle_b);
-    gbVec2 joint1 = {
-        .x = control->arm_a * gb_cos(angle_a),
-        .y = control->arm_a * gb_sin(angle_a)
-    };
-    gbVec2 joint2 = {
-        .x = control->arm_b * gb_cos(angle_b),
-        .y = control->arm_b * gb_sin(angle_b)
-    };
-    gb_vec2_add(point, joint1, joint2);
-}
-
-
-int controller_calculate_velocity(controller_t *control)
-{
-    uint32_t setpoint_time;
-    uint32_t ik_time;
-    uint32_t angle_time;
-    uint32_t velocity_time;
-    uint32_t start = xtimer_now_usec();
-    gbVec2 next_setpoint;
-    int status = controller_sub_setpoint(control, &next_setpoint, 0.005);
-    setpoint_time = xtimer_now_usec();
-    float angle1;
-    float angle2;
-    controller_inverse_kinematics(control, &next_setpoint, &angle1, &angle2);
-    ik_time = xtimer_now_usec();
-    float angle_a;
-    stepper_get_angle(control->stepper_a, &angle_a);
-    float angle_b;
-    stepper_get_angle(control->stepper_b, &angle_b);
-    angle_time = xtimer_now_usec();
-    //printf("angle1: %f, angle2 %f\n", angle1, angle2);
-    //printf("anglea: %f, angleb %f\n", angle_a, angle_b);
-    float omega1 = (angle1-angle_a)*25;
-    float omega2 = (angle2-angle_b)*25;
-    if (status == CONTROLLER_STATISFIED)
-    {
-        omega1 = 0.0;
-        omega2 = 0.0;
-    }
-   // printf("omega1: %f, omega2 %f\n", omega1, omega2);
-    stepper_set_velocity(control->stepper_a, omega1);
-    stepper_set_velocity(control->stepper_b, omega2);
-    velocity_time = xtimer_now_usec();
-    printf("Setpoint: %lu\n", setpoint_time - start);
-    printf("IK-time: %lu\n", ik_time - setpoint_time);
-    printf("Angles: %lu\n", angle_time - ik_time);
-    printf("velocity: %lu\n", velocity_time - angle_time);
-    return 0;
-}
-
-int controller_sub_setpoint(controller_t *control, gbVec2 *setpoint, float distance)
-{
-    // TODO: if the length is shorten than the distance, do not scale it up
-    // TODO: if the length is zero. Do nothing.
-    // get current position.
-    gbVec2 current_position;
-
-    controller_current_position(control, &current_position);
-    // print_point(&current_position);
-
-    // compare current position with setpoint.
-    //
-    while(1){
-        gbVec2 position_error;
-        path_node_t *next_point = path_next_point(control->path);
-        //gb_vec2_sub(&position_error, next_point->point, current_position);
-        float error_magnitude = gb_vec2_mag(position_error);
-        gbVec2 scaled_position_error;
-        //printf("Magnitude: %f Error ", error_magnitude);
-        //print_point(&position_error);
-        if (error_magnitude > distance)
-        {
-            gb_vec2_div(&scaled_position_error, position_error, error_magnitude/distance);
-            gb_vec2_add(setpoint, current_position, scaled_position_error);
-            //printf("Next ");
-            //print_point(setpoint);
-            return CONTROLLER_OK;
-        }
-        else if (next_point->list.next == NULL)
-        {
-            scaled_position_error = current_position;
-            gb_vec2_add(setpoint, current_position, scaled_position_error);
-            //printf("No new point\n");
-            if (error_magnitude < distance/8)
-            {
-                return CONTROLLER_STATISFIED;
-            }
-            return CONTROLLER_NO_PATH;
-        }
-        else 
-        {
-            path_increment(control->path);
-            printf("Increment Path\n");
-        }
-    }
-    gbVec2 position_error;
-      gb_vec2_sub(&position_error, control->setpoint, current_position);
-    // scale the length of the error.
-    // to limit the length.
-    control->error = gb_vec2_mag(position_error);
-    gbVec2 scaled_position_error;
-    if (control->error > distance) {
-    gb_vec2_div(&scaled_position_error, position_error, control->error/distance);
-    } else {
-    scaled_position_error = position_error;
-    }
-    gb_vec2_add(setpoint, current_position, scaled_position_error);
-    return 0;
-}
-*/
 int controller_home(controller_t *control){
     stepper_disable(control->stepper_b);
     stepper_disable(control->stepper_a);
@@ -227,11 +81,21 @@ int controller_home(controller_t *control){
     return 0;
 }
 
-
-int controller_lift_marker(controller_t *control, bool lift){
-    (void)lift;
-    (void)control;
-    return 0;
+void controller_raise_marker(controller_t *control)
+{
+    servo_set(control->servo,SERVO_RAISED);
+    if(!control->lifted){
+        xtimer_usleep(100000);
+        control->lifted = true;
+    }
 }
     
+void controller_lower_marker(controller_t *control)
+{
+    servo_set(control->servo,SERVO_LOWERED);
+    if(control->lifted){
+        xtimer_usleep(100000);
+        control->lifted = false;
+    }
+}
 
diff --git a/implementation/software/include/controller.h b/implementation/software/include/controller.h
index 3284f63..96b5180 100644
--- a/implementation/software/include/controller.h
+++ b/implementation/software/include/controller.h
@@ -4,10 +4,18 @@
 #include "event.h"
 #include "gb_math.h"
 #include "stepper.h"
+#include "servo.h"
 #include "path.h"
 #include "angle.h"
 
 
+#ifndef SERVO_RAISED
+#define SERVO_RAISED (500U)
+#endif
+#ifndef SERVO_LOWERED
+#define SERVO_LOWERED (750U)
+#endif
+
 #define VEL_DIV 1024
 
 #ifdef __cplusplus
@@ -25,6 +33,8 @@ typedef struct {
     stepper_t *stepper_a;
     stepper_t *stepper_b;
     uint32_t time_step;
+    servo_t *servo;
+    bool lifted;
     path_t *path;
     thread_flags_t flags;
 } controller_t;
@@ -43,19 +53,6 @@ int controller_init(controller_t *control);
  */
 int controller_loop(controller_t *control);
 
-//void controller_update(event_t *event);
-
-//int controller_setpoint(controller_t *control, gbVec2 *point);
-//
-//void controller_inverse_kinematics(controller_t *control, gbVec2 *point, float *angle1, float *angle2);
-//
-//void controller_current_position(controller_t *control, gbVec2 *point);
-//
-//int controller_calculate_velocity(controller_t *control);
-//
-//int controller_sub_setpoint(controller_t *control, gbVec2 *setpoint, float distance);
-
-
 /**
  * @brief home both stepper motors
  *
@@ -63,7 +60,19 @@ int controller_loop(controller_t *control);
  */
 int controller_home(controller_t *control);
 
+/**
+ * @brief lower the marker position
+ *
+ * @param[in] control    struct describing the controller
+ */
+void controller_lower_marker(controller_t *control);
 
+/**
+ * @brief raise the marker position
+ *
+ * @param[in] control    struct describing the controller
+ */
+void controller_raise_marker(controller_t *control);
 
 
 #ifdef __cplusplus
diff --git a/implementation/software/include/path.h b/implementation/software/include/path.h
index bf5781f..93afb3f 100644
--- a/implementation/software/include/path.h
+++ b/implementation/software/include/path.h
@@ -7,6 +7,20 @@
 #include "asteroids_font.h"
 #include "xtimer.h"
 
+#ifndef STEPMULTI_X
+#define STEPMULTI_X (4)
+#endif
+#ifndef CHAR_OFFSET_X
+#define CHAR_OFFSET_X (40)
+#endif
+#ifndef STEPMULTI_Y
+#define STEPMULTI_Y (5)
+#endif
+#ifndef CHAR_OFFSET_Y
+#define CHAR_OFFSET_Y (20)
+#endif
+
+
 typedef struct{
     struct {int16_t x, y; };
     int16_t e[2];
@@ -36,24 +50,56 @@ typedef struct{
     uint8_t char_count;
 } path_t;
 
+/** @brief Initialize path
+ *
+ * @param[out] path     struct describing the path
+ */
+void path_init(path_t *path);
+
+/** @brief add a point to the path, 
+ * @note Always use path_add_vec if possible
+ *
+ * @param[in] path     struct describing the path
+ */
 void path_add(path_t *path, path_node_t *point);
 
+/** @brief add a point with modus
+ *
+ * @param[in] path     struct describing the path
+ */
 void path_add_vec(path_t *node, point_t *point, path_modus_t modus);
 
-//void path_add_rect(path_t *node, gbRect2 *rect);
-
+/** @brief add a point to the path
+ *
+ * @param[in] path     struct describing the path
+ */
 void path_generate(path_t *path);
 
+/** @brief reset path point to starting node
+ *
+ * @param[in] path     struct describing the path
+ */
 void path_reset(path_t *path);
 
-void path_add_circle(path_t *node, gbVec2 *point, float radius, uint8_t division);
-
-void path_reverse(path_t *node);
-
+/** @brief reverse the points in the given path
+ *
+ * @param[in] path     struct describing the path
+ */
+void path_reverse(path_t *path);
+
+/** @brief increment the current node to the next in the path list.
+ * Does nothing if already pointing to the last node.
+ *
+ * @param[in] path     struct describing the path
+ */
 void path_increment(path_t *path);
 
-void path_init(path_t *path);
-
+/** 
+ * @brief add a ascii character to the path
+ *
+ * @param[in] path     struct describing the path
+ * @param[in] char     ascii character
+ */
 void path_add_char(path_t *path, char character);
 
 path_node_t* path_next_point(path_t *path);
diff --git a/implementation/software/main.c b/implementation/software/main.c
index 29c4d37..3bf0a71 100644
--- a/implementation/software/main.c
+++ b/implementation/software/main.c
@@ -58,100 +58,20 @@ static void *claiming_thread(void *arg)
     return NULL;
 }
 
-/*int main(void)
-{
-    event_queue_t event_queue = EVENT_QUEUE_INIT_DETACHED;
-    event_queue_init_detached(&event_queue);
-    thread_create(stack, sizeof(stack), PRIO, 0, claiming_thread, &event_queue, "ct");
-    event_t event1 = {.handler = set1};
-    event_t event2 = {.handler = set1};
-    thread_t *main_thread = thread_get_active();
-    test_t test1 = {
-        .event = event1,
-        .thread = main_thread,
-        .flags = 0x1
-    };
-    test_t test2 = {
-        .event = event2,
-        .thread = main_thread,
-        .flags = 0x2
-    };
-    event_timeout_t timeout1;
-    event_timeout_t timeout2;
-    event_timeout_init(&timeout1, &event_queue, (event_t*)&test1);
-    event_timeout_init(&timeout2, &event_queue, (event_t*)&test2);
-    event_timeout_set(&timeout1, 1000000);
-    event_timeout_set(&timeout2, 3000000);
-    printf("Timeouts are set\n");
-    thread_flags_wait_all(0x3);
-    printf("Flags are set\n");
-}*/
-
-
-/*
 int main(void)
 {
-    event_queue_init(&queue);
-    stepper_t stepper_a;
-    stepper_a.queue = &queue;
-    stepper_init(&stepper_a, &stepper_params[0]);
-    test_velocity(&stepper_a,800);
-    //stepper_set_velocity(&stepper_a, 20);
-    //event_loop(&queue);
-    stepper_disable(&stepper_a);
-    return 0;
-}
-*/
-
-/*int main(void)
-{
-
-    stepper_t stepper_a;
-    stepper_t stepper_b;
-    controller_t controller_scara;
-    controller_scara.stepper_a = &stepper_a;
-    controller_scara.stepper_b = &stepper_b;
-    controller_scara.queue = &queue;
-    controller_scara.time_step = 10000;
-    controller_scara.arm_a = 0.070;
-    controller_scara.arm_b = 0.055;
-    gbVec2 setpoint = {
-        .x = 0.015,
-        .y = 0.085,
-    };
-    float anglea;
-    float angleb;
-    controller_inverse_kinematics(&controller_scara, &setpoint, &anglea, &angleb);
-    printf("Angle A: %f, B: %f\n", anglea,angleb);
-}
-*/
-
-int main(void)
-{
-    //printf("Frequentie: %lu Hz\n");
-    servo_t servo;
-    int servo_ret = servo_init(&servo, PWM_DEV(0), 2, 0, 1000);
-    printf("Servo_set: %i\n", servo_ret);
-    servo_set(&servo,350);
-    xtimer_usleep(1000000);
-    servo_set(&servo,750);
-    xtimer_usleep(1000000);
-    return 0;
-
-    
     event_queue_t queue = EVENT_QUEUE_INIT_DETACHED;
     event_queue_init_detached(&queue);
     thread_create(stack, sizeof(stack), PRIO, 0, claiming_thread, &queue, "ct");
 
     thread_t* main_thread = thread_get_active();
 
+    servo_t servo;
+    servo_init(&servo, PWM_DEV(0), 2, 0, 1000);
+
 
     path_t path;
     path_init(&path);
-    //point_t point = {.x = 0, .y = 0};
-    //path_add_vec(&path, &point, PATH_DRAW);
-    //point_t point1= {.x = 72, .y = 90};
-    //path_add_vec(&path, &point1, PATH_DRAW);
     path_add_char(&path, 'S');
     path_add_char(&path, 'Q');
     path_add_char(&path, 'X');
@@ -159,7 +79,6 @@ int main(void)
     path_add_vec(&path, &point1, PATH_MOVE);
     point_t point2= {.x = 72, .y = 89};
     path_add_vec(&path, &point2, PATH_MOVE);
-    //path_add_char(&path, '!');
     path_reverse(&path);
     path_reset(&path);
     
@@ -187,17 +106,12 @@ int main(void)
     controller_t controller_scara;
     controller_scara.stepper_a = &stepper_a;
     controller_scara.stepper_b = &stepper_b;
+    controller_scara.servo = &servo;
     controller_scara.time_step = 4000;
     controller_scara.path = &path;
     controller_init(&controller_scara);
-    for (int i = 0; i < 50; i++){
     controller_loop(&controller_scara);
-    }
-    printf("einde\n");
-
-
-
-    //event_loop(&queue);
+    
     return 0;
 }
 
diff --git a/implementation/software/path.c b/implementation/software/path.c
index 05084f7..a87dcaf 100644
--- a/implementation/software/path.c
+++ b/implementation/software/path.c
@@ -1,19 +1,16 @@
 #include "include/path.h"
 
-#define STEPMULTI_X (4)
-#define CHAR_OFFSET_X (40)
-#define STEPMULTI_Y (5)
-#define CHAR_OFFSET_Y (20)
 
-
-void pp(point_t *point)
+void path_init(path_t *path)
 {
-    printf("%i, %i\n", point->x, point->y);
+    path->current = path->start;
+    path->char_count = 0;
+    point_t origin = {.x = 0, .y = 0};
+    path_add_vec(path, &origin, PATH_MOVE);
 }
 
 void path_add(path_t *path, path_node_t *point){
     list_add(&path->start, &point->list);
-    pp(&point->point);
 }
 
 void path_add_vec(path_t *node, point_t *point, path_modus_t modus){
@@ -30,8 +27,6 @@ void path_add_interpolated(path_t *path, point_t *point, path_modus_t modus)
         return;
     }
     path_node_t* current_node = (path_node_t*)path->start.next;
-    //printf("Current node @ ");
-    //pp(&current_node->point);
     point_t point_diff = point_sub(point, &current_node->point);
     while(point_mag2(&point_diff) > 64 )
     {
@@ -43,25 +38,16 @@ void path_add_interpolated(path_t *path, point_t *point, path_modus_t modus)
     path_add_vec(path, point, modus);
 }
 
-/*
-void path_add_rect(path_t *node, gbRect2 *rect){
-    gbVec2 corner = rect->pos;
-    path_add_vec(node, &corner, PATH_MOVE);
-    corner.x = corner.x + rect->dim.x;
-    path_add_vec(node, &corner, PATH_DRAW);
-    gb_vec2_add(&corner, rect->pos, rect->dim);
-    path_add_vec(node, &corner, PATH_DRAW);
-    corner.x = corner.x - rect->dim.x;
-    path_add_vec(node, &corner, PATH_DRAW);
-    corner = rect->pos;
-    path_add_vec(node, &corner, PATH_DRAW);
-}
-*/
-
 void path_add_char(path_t *path, char character)
 {
     path_modus_t modus = PATH_MOVE;
-    asteroids_char_t char_path = asteroids_font[(uint8_t)character - 0x20];
+    asteroids_char_t char_path;
+    if (character >= 'a' && character <= 'z')
+    {
+        char_path = asteroids_font[(uint8_t)character - 0x40];
+    } else {
+        char_path = asteroids_font[(uint8_t)character - 0x20];
+    }
     for(int i = 0; i<8; i++)
     {
         uint8_t char_point = char_path.points[i];
@@ -83,14 +69,6 @@ void path_add_char(path_t *path, char character)
     path->char_count++;
 }
 
-void path_init(path_t *path)
-{
-    path->current = path->start;
-    path->char_count = 0;
-    point_t origin = {.x = 0, .y = 0};
-    path_add_vec(path, &origin, PATH_MOVE);
-}
-
 void path_reset(path_t *path)
 {
     path->current = path->start;
@@ -107,12 +85,12 @@ void path_add_arc(path_t *node, gbVec2 *point, float radius, float angle_start,
 }
 */
 
-void path_reverse(path_t *node){
-    if (node->start.next == 0x0)
+void path_reverse(path_t *path){
+    if (path->start.next == 0x0)
     {
         return;
     }
-    list_node_t* curr_node = node->start.next;
+    list_node_t* curr_node = path->start.next;
     list_node_t* next_node = NULL;
     list_node_t* prev_node = NULL;
     while(curr_node){
@@ -121,7 +99,7 @@ void path_reverse(path_t *node){
         prev_node = curr_node;
         curr_node = next_node;
     }
-    node->start.next = prev_node;
+    path->start.next = prev_node;
 }
 
 void path_increment(path_t *path)