diff --git a/Inc/cube.h b/Inc/cube.h
new file mode 100644
index 0000000..793ac86
--- /dev/null
+++ b/Inc/cube.h
@@ -0,0 +1,13 @@
+//#include <string.h>
+#include <math.h>
+//#include "stm32f4xx_hal.h"
+#include "ssd1306.h"
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+} triPoint;
+
+void cube_Init(void);
+void cube_Update(uint8_t fov);
diff --git a/Inc/starfield.h b/Inc/starfield.h
new file mode 100644
index 0000000..6a1afbf
--- /dev/null
+++ b/Inc/starfield.h
@@ -0,0 +1,15 @@
+#include <string.h>
+//#include "stm32f4xx_hal.h"
+#include "ssd1306.h"
+
+struct Star
+{
+    unsigned int x;
+    unsigned int y;
+    unsigned int type;
+};
+
+#define MAX_STARS 80
+
+void starfield_Init(void);
+void starfield_Update(void);
diff --git a/Makefile b/Makefile
index a13ab04..bbe4bca 100644
--- a/Makefile
+++ b/Makefile
@@ -26,6 +26,8 @@ BUILD_DIR = build
 ######################################
 C_SOURCES = \
   Src/main.c \
+  Src/cube.c \
+  Src/starfield.c \
   Src/ssd1306.c \
   Src/fonts.c \
   Src/system_stm32f4xx.c \
diff --git a/Src/cube.c b/Src/cube.c
new file mode 100644
index 0000000..e8c9a96
--- /dev/null
+++ b/Src/cube.c
@@ -0,0 +1,106 @@
+#include "cube.h"
+
+triPoint vertices[8];
+triPoint lineVertices[8];
+
+uint32_t angleX = 0;
+uint32_t angleY = 0;
+uint32_t angleZ = 0;
+
+void cube_Init() {
+    vertices[0].x = -1;
+    vertices[0].y = 1;
+    vertices[0].z = -1;
+    vertices[1].x = 1;
+    vertices[1].y = 1;
+    vertices[1].z = -1;
+    vertices[2].x = 1;
+    vertices[2].y = -1;
+    vertices[2].z = -1;
+    vertices[3].x = -1;
+    vertices[3].y = -1;
+    vertices[3].z = -1;
+    vertices[4].x = -1;
+    vertices[4].y = 1;
+    vertices[4].z = 1;
+    vertices[5].x = 1;
+    vertices[5].y = 1;
+    vertices[5].z = 1;
+    vertices[6].x = 1;
+    vertices[6].y = -1;
+    vertices[6].z = 1;
+    vertices[7].x = -1;
+    vertices[7].y = -1;
+    vertices[7].z = 1;
+}
+
+triPoint rotateX(triPoint startingPoint, uint16_t angle) {
+    triPoint computedPoint;
+    float rad = angle * 3.141592 / 180;
+    float cosa = cos(rad);
+    float sina = sin(rad);
+
+    computedPoint.x = startingPoint.x;
+    computedPoint.y = (startingPoint.y * cosa) - (startingPoint.z * sina);
+    computedPoint.z = (startingPoint.y * sina) + (startingPoint.z * cosa);
+    return computedPoint;
+}
+
+triPoint rotateY(triPoint startingPoint, uint16_t angle) {
+    triPoint computedPoint;
+    float rad = angle * 3.141592 / 180;
+    float cosa = cos(rad);
+    float sina = sin(rad);
+
+    computedPoint.y = startingPoint.y;
+    computedPoint.z = (startingPoint.z * cosa) - (startingPoint.x * sina);
+    computedPoint.x = (startingPoint.z * sina) + (startingPoint.x * cosa);
+    return computedPoint;
+}
+
+triPoint rotateZ(triPoint startingPoint, uint16_t angle) {
+    triPoint computedPoint;
+    float rad = angle * 3.141592 / 180;
+    float cosa = cos(rad);
+    float sina = sin(rad);
+
+    computedPoint.z = startingPoint.z;
+    computedPoint.x = (startingPoint.x * cosa) - (startingPoint.y * sina);
+    computedPoint.y = (startingPoint.x * sina) + (startingPoint.y * cosa);
+    return computedPoint;
+}
+
+triPoint projectPoint(triPoint startingPoint, uint8_t win_width, uint8_t win_height, uint8_t fov, uint8_t viewer_distance) {
+    triPoint returnPoint;
+    float factor = fov/(viewer_distance + startingPoint.z);
+
+    returnPoint.x = startingPoint.x * factor + win_width/2;
+    returnPoint.y = -startingPoint.y * factor + win_height/2;
+    returnPoint.z = 1;
+    return returnPoint;
+}
+
+void cube_Update(uint8_t fov) {
+    triPoint calcPoint, newPoint;
+
+    for (uint8_t i = 0; i < 8; i++) {
+        calcPoint = rotateZ(vertices[i],angleZ++);
+        calcPoint = rotateY(calcPoint,angleY++);
+        calcPoint = rotateX(calcPoint,angleX++);
+        newPoint = projectPoint(calcPoint, SSD1306_WIDTH, SSD1306_HEIGHT, fov, 5);
+        lineVertices[i] = newPoint;
+    }
+
+    drawLine(lineVertices[0].x,lineVertices[0].y,lineVertices[1].x,lineVertices[1].y);
+    drawLine(lineVertices[1].x,lineVertices[1].y,lineVertices[2].x,lineVertices[2].y);
+    drawLine(lineVertices[2].x,lineVertices[2].y,lineVertices[3].x,lineVertices[3].y);
+    drawLine(lineVertices[3].x,lineVertices[3].y,lineVertices[0].x,lineVertices[0].y);
+    drawLine(lineVertices[0].x,lineVertices[0].y,lineVertices[4].x,lineVertices[4].y);
+    drawLine(lineVertices[1].x,lineVertices[1].y,lineVertices[5].x,lineVertices[5].y);
+    drawLine(lineVertices[2].x,lineVertices[2].y,lineVertices[6].x,lineVertices[6].y);
+    drawLine(lineVertices[3].x,lineVertices[3].y,lineVertices[7].x,lineVertices[7].y);
+    drawLine(lineVertices[4].x,lineVertices[4].y,lineVertices[5].x,lineVertices[5].y);
+    drawLine(lineVertices[5].x,lineVertices[5].y,lineVertices[6].x,lineVertices[6].y);
+    drawLine(lineVertices[6].x,lineVertices[6].y,lineVertices[7].x,lineVertices[7].y);
+    drawLine(lineVertices[7].x,lineVertices[7].y,lineVertices[4].x,lineVertices[4].y);
+}
diff --git a/Src/main.c b/Src/main.c
index 07bb92b..38532c3 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -31,9 +31,7 @@
   ******************************************************************************
   */
 /* Includes ------------------------------------------------------------------*/
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
+//#include <string.h>
 
 #include "main.h"
 #include "stm32f4xx_hal.h"
@@ -41,6 +39,8 @@
 /* USER CODE BEGIN Includes */
 #include "ssd1306.h"
 #include "fonts.h"
+#include "starfield.h"
+#include "cube.h"
 //#include "dht22.h"
 /* USER CODE END Includes */
 
@@ -54,30 +54,6 @@ char UART1_Data;
 char UART_Buffer[32];
 char text[32];
 int i;
-
-typedef struct {
-  float x;
-  float y;
-  float z;
-} triPoint;
-
-triPoint vertices[8];
-triPoint lineVertices[8];
-
-uint32_t angleX = 0;
-uint32_t angleY = 0;
-uint32_t angleZ = 0;
-
-struct _tStar
-{
-  unsigned char nX;
-  unsigned char nY;
-  unsigned char nStarType;
-};
-
-#define STARS_NUM 80
-struct _tStar aStars[STARS_NUM];
-
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -93,92 +69,11 @@ static void MX_USART1_UART_Init(void);
 /* USER CODE END PFP */
 
 /* USER CODE BEGIN 0 */
-triPoint rotateX(triPoint startingPoint, uint16_t angle) {
-  triPoint computedPoint;
-
-  float rad = angle * 3.141592 / 180;
-  float cosa = cos(rad);
-  float sina = sin(rad);
-  computedPoint.x = startingPoint.x;
-  computedPoint.y = (startingPoint.y * cosa) - (startingPoint.z * sina);
-  computedPoint.z = (startingPoint.y * sina) + (startingPoint.z * cosa);
-  return computedPoint;
-}
-
-triPoint rotateY(triPoint startingPoint, uint16_t angle) {
-  triPoint computedPoint;
-
-  float rad = angle * 3.141592 / 180;
-  float cosa = cos(rad);
-  float sina = sin(rad);
-  computedPoint.y = startingPoint.y;
-  computedPoint.z = (startingPoint.z * cosa) - (startingPoint.x * sina);
-  computedPoint.x = (startingPoint.z * sina) + (startingPoint.x * cosa);
-  return computedPoint;
-}
-
-triPoint rotateZ(triPoint startingPoint, uint16_t angle) {
-  triPoint computedPoint;
-
-  float rad = angle * 3.141592 / 180;
-  float cosa = cos(rad);
-  float sina = sin(rad);
-  computedPoint.z = startingPoint.z;
-  computedPoint.x = (startingPoint.x * cosa) - (startingPoint.y * sina);
-  computedPoint.y = (startingPoint.x * sina) + (startingPoint.y * cosa);
-  return computedPoint;
-}
-
-triPoint projectPoint(triPoint startingPoint, uint8_t win_width, uint8_t win_height, uint8_t fov, uint8_t viewer_distance) {
-  triPoint returnPoint;
-  float factor = fov/(viewer_distance + startingPoint.z);
-  returnPoint.x = startingPoint.x * factor + win_width/2;
-  returnPoint.y = -startingPoint.y * factor + win_height/2;
-  returnPoint.z = 1;
-  return returnPoint;
-}
 /* USER CODE END 0 */
 
 int main(void)
 {
   /* USER CODE BEGIN 1 */
-  vertices[0].x = -1;
-  vertices[0].y = 1;
-  vertices[0].z = -1;
-  vertices[1].x = 1;
-  vertices[1].y = 1;
-  vertices[1].z = -1;
-  vertices[2].x = 1;
-  vertices[2].y = -1;
-  vertices[2].z = -1;
-  vertices[3].x = -1;
-  vertices[3].y = -1;
-  vertices[3].z = -1;
-  vertices[4].x = -1;
-  vertices[4].y = 1;
-  vertices[4].z = 1;
-  vertices[5].x = 1;
-  vertices[5].y = 1;
-  vertices[5].z = 1;
-  vertices[6].x = 1;
-  vertices[6].y = -1;
-  vertices[6].z = 1;
-  vertices[7].x = -1;
-  vertices[7].y = -1;
-  vertices[7].z = 1;
-
-  triPoint calcPoint, newPoint;
-
-  unsigned char nStar = 0;
-  memset(aStars, 0, sizeof(aStars));
-
-  //Init
-  for(nStar = 0; nStar < STARS_NUM; nStar++)
-  {
-    aStars[nStar].nX = rand() % 128;
-    aStars[nStar].nY = rand() % 64;
-    aStars[nStar].nStarType = rand() % 3;
-  }
   /* USER CODE END 1 */
 
   /* MCU Configuration----------------------------------------------------------*/
@@ -195,29 +90,21 @@ int main(void)
 
   /* USER CODE BEGIN 2 */
   ssd1306_Init();
+  starfield_Init();
+  cube_Init();
   //DHT22_Init();
   HAL_UART_Receive_IT(&huart1, (uint8_t*) &UART1_Data, 1);
-
-/*
-  for(int y = 0; y < 129; y++) {
-    ssd1306_DrawPixel(y, 20, White);
-  }
-*/
-  //drawLine(1, 1, 20, 20);
-  //ssd1306_UpdateScreen();
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
-  int zbr = 40;
-  int direction = 1;
   while (1)
   {
   /* USER CODE END WHILE */
 
   /* USER CODE BEGIN 3 */
     ssd1306_Fill(Black);
-    //ssd1306_UpdateScreen();
+
     /*
     int x, msg;
     HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_6); //Toggle the state of pin PC9
@@ -238,77 +125,11 @@ int main(void)
     ssd1306_WriteString((msg == 1) ? "GORDOS" : "LOL", Font_7x10, White);
 */
 
-    for(nStar = 0; nStar < STARS_NUM; nStar++) {
-      //move star
-      switch(aStars[nStar].nStarType) {
-        case 0: //slow star
-          aStars[nStar].nX += 2;
-          break;
-        case 1: //medium star
-          aStars[nStar].nX += 3;
-          break;
-        case 2: //fast star
-          aStars[nStar].nX += 4;
-          break;
-      }
-
-      if (aStars[nStar].nX >= 160) {
-        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_6); //Toggle the state of pin PC9
-        aStars[nStar].nX = 0;
-        aStars[nStar].nY = rand() % 200;
-        aStars[nStar].nStarType = rand() % 3;
-        continue;
-      }
-
-      //paint star
-      ssd1306_DrawPixel(aStars[nStar].nX, aStars[nStar].nY, White);
-    }
-/*
-    ssd1306_SetCursor(zbr - 39, (sin(zbr) * 10) + 10);
-    ssd1306_WriteString("LOLGORDOS", Font_7x10, White);
-
-    //ssd1306_SetCursor(zbr - 39, 53);
-    ssd1306_SetCursor(zbr - 39, (sin(zbr) * 10) + 43);
-    ssd1306_WriteString("DEMOSCENE", Font_7x10, White);
-*/
-
-    if (direction == 1) {
-      zbr++;
-    } else {
-      zbr--;
-    }
-
-    for (uint8_t i = 0; i<8; i++) {
-      calcPoint = rotateZ(vertices[i],angleZ++);
-      calcPoint = rotateY(calcPoint,angleY++);
-      calcPoint = rotateX(calcPoint,angleX++);
-      newPoint = projectPoint(calcPoint, 128, 64, zbr, 5);
-      lineVertices[i] = newPoint;
-    }
-
-    drawLine(lineVertices[0].x,lineVertices[0].y,lineVertices[1].x,lineVertices[1].y);
-    drawLine(lineVertices[1].x,lineVertices[1].y,lineVertices[2].x,lineVertices[2].y);
-    drawLine(lineVertices[2].x,lineVertices[2].y,lineVertices[3].x,lineVertices[3].y);
-    drawLine(lineVertices[3].x,lineVertices[3].y,lineVertices[0].x,lineVertices[0].y);
-    drawLine(lineVertices[0].x,lineVertices[0].y,lineVertices[4].x,lineVertices[4].y);
-    drawLine(lineVertices[1].x,lineVertices[1].y,lineVertices[5].x,lineVertices[5].y);
-    drawLine(lineVertices[2].x,lineVertices[2].y,lineVertices[6].x,lineVertices[6].y);
-    drawLine(lineVertices[3].x,lineVertices[3].y,lineVertices[7].x,lineVertices[7].y);
-    drawLine(lineVertices[4].x,lineVertices[4].y,lineVertices[5].x,lineVertices[5].y);
-    drawLine(lineVertices[5].x,lineVertices[5].y,lineVertices[6].x,lineVertices[6].y);
-    drawLine(lineVertices[6].x,lineVertices[6].y,lineVertices[7].x,lineVertices[7].y);
-    drawLine(lineVertices[7].x,lineVertices[7].y,lineVertices[4].x,lineVertices[4].y);
+    starfield_Update();
+    cube_Update(60);
 
     ssd1306_UpdateScreen();
     HAL_Delay(25);
-
-    if (zbr > 100) {
-      direction = 0;
-    }
-
-    if (zbr < 40) {
-      direction = 1;
-    }
   }
   /* USER CODE END 3 */
 }
diff --git a/Src/ssd1306.c b/Src/ssd1306.c
index a9090fa..246bb37 100644
--- a/Src/ssd1306.c
+++ b/Src/ssd1306.c
@@ -1,6 +1,5 @@
 #include"ssd1306.h"
 
-
 static uint8_t SSD1306_Buffer[SSD1306_HEIGHT * SSD1306_WIDTH / 8];
 
 // Een scherm-object om lokaal in te werken
diff --git a/Src/starfield.c b/Src/starfield.c
new file mode 100644
index 0000000..6deb3d0
--- /dev/null
+++ b/Src/starfield.c
@@ -0,0 +1,44 @@
+#include"starfield.h"
+
+struct Star Stars[MAX_STARS];
+
+void starfield_Init(void) {
+    unsigned char theStar = 0;
+    memset(Stars, 0, sizeof(Stars));
+
+    for(theStar = 0; theStar < MAX_STARS; theStar++) {
+        Stars[theStar].x = rand() % SSD1306_WIDTH;
+        Stars[theStar].y = rand() % SSD1306_HEIGHT;
+        Stars[theStar].type = rand() % 3;
+    }
+}
+
+void starfield_Update(void) {
+    unsigned char theStar = 0;
+
+    for(theStar = 0; theStar < MAX_STARS; theStar++) {
+
+        switch(Stars[theStar].type) {
+            case 0:
+                Stars[theStar].x += 2;
+                break;
+            case 1:
+                Stars[theStar].x += 3;
+                break;
+            case 2:
+                Stars[theStar].x += 4;
+            break;
+        }
+
+        if (Stars[theStar].x >= SSD1306_WIDTH) {
+            //HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_6);
+            Stars[theStar].x = 0;
+            Stars[theStar].y = rand() % SSD1306_HEIGHT;
+            Stars[theStar].type = rand() % 3;
+            continue;
+        }
+
+        // Draw star
+        ssd1306_DrawPixel(Stars[theStar].x, Stars[theStar].y, White);
+    }
+}