diff --git a/project.ino b/project.ino index 2dd8e52..98912f3 100644 --- a/project.ino +++ b/project.ino @@ -25,6 +25,7 @@ float x,y; class Vec4 { public: Vec4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w) {} + Vec4() = default; Vec4 to_rotation(){ float base_angle = atan2(x, y); @@ -70,25 +71,44 @@ public: float x,y,z,w; }; -// communication -Vec4 read() { - float x = parseFloat(); - float y = parseFloat(); - float z = parseFloat(); - float w = parseFloat(); - return Vec4(x,y,z,w); -} - void setup() { Serial.begin(9600); Braccio.begin(); - pinPeripheral(PIN_SERIAL1_TX, PIO_SERCOM); - pinPeripheral(PIN_SERIAL1_RX, PIO_SERCOM); } +Vec4 targ; +const int buffer_size = 16; +byte in[buffer_size]; + + void loop() { - Vec4 targ = read(); + int amogos = 0; + while(true){ + if (Serial.available() >= 4){ + Serial.readBytes(in, buffer_size); + float decoded = byte(in); + float vec[3]; + vec[0] = targ.x; + vec[1] = targ.y; + vec[2] = targ.z; + vec[3] = targ.w; + + vec[amogos] = decoded; + + targ = Vec4(vec[0],vec[1],vec[2],vec[3]); + amogos++; + if(amogos == 4){ + break; + } + //float x = Serial.read(); + //float y = Serial.read(); + //float z = Serial.read(); + //float w = Serial.read(); + + } + } + Vec4 rot = targ.to_rotation(); // speed inverse knematics wrist rot grip Braccio.ServoMovement(30, rot.x, rot.y, rot.z, rot.w, 90, 70); -} +} \ No newline at end of file