code from arduino

project.ino

@@ -0,0 +1,115 @@
+#include <Braccio.h>
+#include <Servo.h>
+#include <math.h>
+
+#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) {}
+float x,y;  
+};
+
+class Vec4 {
+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;
+
+    // take the wrist into account
+    Vec2 target_position = Vec2(hypotf(x, y) - sin(wRAD) * wrist_length, -z + cos(wRAD) * wrist_length);
+
+    /////// shoulder and elbow angles calculations ///////
+    
+    // calculate distance beetween the arm base and the target position and then clamp it to 1
+    float target_base_distance = hypotf(target_position.x, target_position.y) / shoulder_plus_elbow_length;
+    if (target_base_distance > 1) {
+      target_base_distance = 1;
+    }
+
+    // set shoulder angle to arm bend angle from distance from target position to base position
+    float shoulder_angle = asin(target_base_distance);
+
+    // calculate shoulder angle from the shoulder angle before performing another calculation on it
+    float elbow_angle = PI - (shoulder_angle * 2);
+
+    // add angle beetween the target position and the base position to the shoulder angle
+    shoulder_angle += atan2(target_position.y, target_position.x);
+
+    ///////// wrist angle calculation ///////
+    float shoulder_plus_elbow_angle = shoulder_angle + elbow_angle; // shoulder angle + elbow angle
+    //Serial.println(shoulder_plus_elbow_angle);
+    //Serial.println(wRAD);
+    float wrist_angle = wRAD - shoulder_plus_elbow_angle; // # target wrist angle - shoulder_plus_elbow_angle   so the wrist angle is always fixes to the base rotation
+    
+    return Vec4(base_angle * 180 / PI, shoulder_angle * 180 / PI + 90, elbow_angle * 180 / PI + 90, wrist_angle * 180 / PI + 90);
+  }
+
+  void print(){
+    //Serial.println(x);
+    Serial.println(y);
+    Serial.println(z);
+    Serial.println(w);
+    Serial.println("==================");
+  }
+  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); 
+  }
+
+}