forked from kacperks/inv-kin-arm
24 lines
469 B
Arduino
24 lines
469 B
Arduino
|
#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);
|
||
|
|
}
|