Looki2000/inv-kin-arm
1
0
Fork 0
forked from kacperks/inv-kin-arm
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Test

Browse Source
This commit is contained in:
Kostka 2022-10-07 12:52:05 +02:00
parent 46673402a2
commit 408bcb1def
3 changed files with 26 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
arduino_robotic_arm.ino Normal file
View File

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

0
controll.hpp Normal file
View File

55
project.ino → math.hpp
View File

@ -1,22 +1,14 @@
#include <Braccio.h> #ifndef ROBOT_MATH_HPP
#include <Servo.h> #define ROBOT_MATH_HPP
#include <math.h>
#define PI 3.14159265359 #define PI 3.14159265359
#define PIdiv2 PI/2 #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_and_elbow_length = 125;
const float shoulder_plus_elbow_length = shoulder_and_elbow_length * 2; const float shoulder_plus_elbow_length = shoulder_and_elbow_length * 2;
const float wrist_length = 190; const float wrist_length = 190;
class Vec2 { class Vec2 {
public: public:
Vec2(float _x, float _y) : x(_x), y(_y) {} Vec2(float _x, float _y) : x(_x), y(_y) {}
@ -28,7 +20,6 @@ public:
Vec4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w) {} Vec4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w) {}
Vec4 to_rotation(){ Vec4 to_rotation(){
// DZIAŁA
float base_angle = atan2(x, y); float base_angle = atan2(x, y);
float wRAD = w * PI / 180; float wRAD = w * PI / 180;
@ -72,44 +63,4 @@ public:
float x,y,z,w; float x,y,z,w;
}; };
int xxx = 302; #endif
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);
}
}