Zaktualizuj 'project.ino'

Reviewed-on: http://git.cubesoftware.xyz:20524/kacperks/inv-kin-arm/pulls/1

arduino_robotic_arm.ino

@@ -1,23 +0,0 @@
-#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); 
-}

project.ino

@@ -1,10 +1,17 @@
-#ifndef ROBOT_MATH_HPP
-#define ROBOT_MATH_HPP
+#include <Braccio.h>
+#include <Servo.h>
+#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;
@@ -18,6 +25,7 @@ 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);
@@ -63,4 +71,44 @@ public:
   float x,y,z,w;
 };
 
-#endif
+void setup() {
+  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); 
+}