#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); 
}