#include <stdlib.h>
#include <math.h>
#include "Trajectroy.h"
#include "BangBangTrajectroy.h"
#include "vec.h"
#include "SvrData.h"

template <unsigned SIZE> 
BangBangJointTrajectroy<SIZE>::BangBangJointTrajectroy(
        unsigned int steps_,
        float totalTime_,
        Vec<float,SIZE> jointMovement
        )
{
    Vec< float, SIZE> velocity = jointMovement / steps_ * 1.5f;
    //TODO
    LinearJointTrajectory(
            steps_,
            totalTime_,
            jointMovement,
            velocity);
}