From a072beb9283dd62b3bd9a49b375d9c80b3814f29 Mon Sep 17 00:00:00 2001
From: Kostka <Kacper.Kostka@montessori.gda.pl>
Date: Fri, 21 Oct 2022 12:55:06 +0200
Subject: [PATCH] Serial Port Communication added!

---
 arduino_robotic_arm.ino | 23 -----------------------
 math.hpp => project.ino | 36 ++++++++++++++++++++++++++++++++----
 2 files changed, 32 insertions(+), 27 deletions(-)
 delete mode 100644 arduino_robotic_arm.ino
 rename math.hpp => project.ino (75%)

diff --git a/arduino_robotic_arm.ino b/arduino_robotic_arm.ino
deleted file mode 100644
index 43042ee..0000000
--- a/arduino_robotic_arm.ino
+++ /dev/null
@@ -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); 
-}
\ No newline at end of file
diff --git a/math.hpp b/project.ino
similarity index 75%
rename from math.hpp
rename to project.ino
index 30f6d39..2dd8e52 100644
--- a/math.hpp
+++ b/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;
@@ -63,4 +70,25 @@ public:
   float x,y,z,w;
 };
 
-#endif
\ No newline at end of file
+// 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);
+}
+
+void loop() {
+  Vec4 targ = 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); 
+}