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); 
+}

math.hpp

@@ -1,17 +1,10 @@
-#include <Braccio.h>
+#ifndef ROBOT_MATH_HPP
-#include <Servo.h>
+#define ROBOT_MATH_HPP
-#include <math.h>
-
-Servo base;
-Servo shoulder;
-Servo elbow;
-Servo wrist_rot;
-Servo wrist_ver;
-Servo gripper;
 
 #define PI 3.14159265359
 #define PIdiv2 PI/2
 
+
 const float shoulder_and_elbow_length = 12.5;
 const float shoulder_plus_elbow_length = shoulder_and_elbow_length * 2;
 const float wrist_length = 19;
@@ -25,7 +18,6 @@ float x,y;
 class Vec4 {
 public:
   Vec4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w) {}
-  Vec4() = default;
   
   Vec4 to_rotation(){
     float base_angle = atan2(x, y);
@@ -71,44 +63,4 @@ public:
   float x,y,z,w;
 };
 
-void setup() {
+#endif
-  Serial.begin(9600);
-  Braccio.begin();
-}
-
-Vec4 targ;
-const int buffer_size = 16;
-byte in[buffer_size];
-
-
-void loop() {
-  int amogos = 0;
-  while(true){
-    if (Serial.available() >= 4){
-      Serial.readBytes(in, buffer_size);
-      float decoded = byte(in);
-      float vec[3];
-      vec[0] = targ.x;
-      vec[1] = targ.y;
-      vec[2] = targ.z;
-      vec[3] = targ.w;
-
-      vec[amogos] = decoded;
-      
-      targ = Vec4(vec[0],vec[1],vec[2],vec[3]);
-      amogos++;
-      if(amogos == 4){
-        break;  
-      }
-      //float x = Serial.read();
-      //float y = Serial.read();
-      //float z = Serial.read();
-      //float w = Serial.read();
-
-    }
-  }
-  
-  Vec4 rot =  targ.to_rotation();
-  //                    speed     inverse knematics           wrist rot  grip
-  Braccio.ServoMovement(30,    rot.x, rot.y, rot.z, rot.w,     90,        70); 
-}