#include "bioscara_arm_hardware_driver/uI2C.h"
#include "bioscara_arm_hardware_driver/mJoint.h"
#include <cmath>