115 lines
3.2 KiB
C++
115 lines
3.2 KiB
C++
#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);
|
|
}
|
|
|
|
} |