#include <fstream>

#include<iostream>
#include <eigen3/Eigen/Dense>
#include<stdio.h>
#include "gaitStep.h"
#include "functions.h"
#include "transitHub.h"
using namespace Eigen;
using namespace std;

int main(int arg,char** argg)
{

    robotModel rb;
    transitHub *port=transitHub::getInstance();
    port->portOpen(1,57600);
    float rotateX, rotateY, rotateZ, len;
    while(1)
    {
        int leg;
        cin>>rotateX>> rotateY>>rotateZ>>len>>leg;
        rb.setLeg(rotateX, rotateY, rotateZ, len,leg);
//        cin>>rotateX>> rotateY>>leg;
//        rb.setFoot(rotateX, rotateY,leg);
        rb.showParam(leg);
        motorArray d;
        rb.getData(d);
        port->doLoopTx(d);
    }


    getchar();
    return 0;
}