Zaktualizuj 'project.ino'

project.ino

@@ -25,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);
@@ -70,25 +71,44 @@ public:
   float x,y,z,w;
 };
 
-// communication
-Vec4 read() {
-    float x = parseFloat();
-    float y = parseFloat();
-    float z = parseFloat();
-    float w = parseFloat();
-    return Vec4(x,y,z,w);
-}
-
 void setup() {
   Serial.begin(9600);
   Braccio.begin();
-  pinPeripheral(PIN_SERIAL1_TX, PIO_SERCOM);
-  pinPeripheral(PIN_SERIAL1_RX, PIO_SERCOM);
 }
 
+Vec4 targ;
+const int buffer_size = 16;
+byte in[buffer_size];
+
+
 void loop() {
-  Vec4 targ = read();
+  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); 
-}
+}