diff --git a/arduino_robotic_arm.ino b/arduino_robotic_arm.ino
new file mode 100644
index 0000000..5d649d8
--- /dev/null
+++ b/arduino_robotic_arm.ino
@@ -0,0 +1,23 @@
+#include <Braccio.h>
+#include <Servo.h>
+#include <math.h>
+#include "math.hpp"
+
+Servo base;
+Servo shoulder;
+Servo elbow;
+Servo wrist_rot;
+Servo wrist_ver;
+Servo gripper;
+
+void setup() {
+  Serial.begin(9600);
+  Braccio.begin();
+}
+
+void loop() {
+  Vec4 targ = Vec4(0, 0, 0, 0);
+  Vec4 rot =  targ.to_rotation();
+  //                    speed     inverse knematics           wrist rot  grip
+  Braccio.ServoMovement(30,    rot.x, rot.y, rot.z, rot.w,     90,        70); 
+}
diff --git a/controll.hpp b/controll.hpp
new file mode 100644
index 0000000..e69de29
diff --git a/project.ino b/math.hpp
similarity index 65%
rename from project.ino
rename to math.hpp
index 23fe6c0..35e060d 100644
--- a/project.ino
+++ b/math.hpp
@@ -1,22 +1,14 @@
-#include <Braccio.h>
-#include <Servo.h>
-#include <math.h>
+#ifndef ROBOT_MATH_HPP
+#define ROBOT_MATH_HPP
 
 #define PI 3.14159265359
 #define PIdiv2 PI/2
 
-Servo base;
-Servo shoulder;
-Servo elbow;
-Servo wrist_rot;
-Servo wrist_ver;
-Servo gripper;
 
 const float shoulder_and_elbow_length = 125;
 const float shoulder_plus_elbow_length = shoulder_and_elbow_length * 2;
 const float wrist_length = 190;
 
-
 class Vec2 {
 public:
 Vec2(float _x, float _y) : x(_x), y(_y) {}
@@ -28,7 +20,6 @@ public:
   Vec4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w) {}
   
   Vec4 to_rotation(){
-    // DZIAŁA
     float base_angle = atan2(x, y);
     
     float wRAD = w * PI / 180;
@@ -72,44 +63,4 @@ public:
   float x,y,z,w;
 };
 
-int xxx = 302;
-int yyy = 209;
-
-void setup() {
-  Serial.begin(9600);
-  Braccio.begin();
-
-  
-  
-
-}
-
-void loop() {
-   for(int i = 29; i < 90; i++) {
-          //if (Serial.available() > 0) {
-    //  String data = Serial.readString();
-    //  data.trim();
-    Vec4 targ = Vec4(xxx, 0, yyy, i);
-    Vec4 rot =  targ.to_rotation();
-
-    //Serial.println(i);
-    //rot.print();
-    //                    speed     inverse knematics           wrist rot  grip
-    Braccio.ServoMovement(30,    rot.x, rot.y, rot.z, rot.w,     90,        70); 
-  }
-
-  
-  for(int i = 90; i > 29; i--) {
-          //if (Serial.available() > 0) {
-    //  String data = Serial.readString();
-    //  data.trim();
-    Vec4 targ = Vec4(xxx, 0, yyy, i);
-    Vec4 rot =  targ.to_rotation();
-
-    //Serial.println(i);
-    //rot.print();
-    //                    speed     inverse knematics           wrist rot  grip
-    Braccio.ServoMovement(30,    rot.x, rot.y, rot.z, rot.w,     90,        70); 
-  }
-
-}
\ No newline at end of file
+#endif
\ No newline at end of file