Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

Files at this revision

API Documentation at this revision

Comitter:
je310
Date:
Mon Oct 15 18:30:20 2018 +0000
Parent:
4:778bc352c47f
Commit message:
testing eigen;

Changed in this revision

Axis.cpp Show annotated file Show diff for this revision Revisions of this file
Axis.h Show annotated file Show diff for this revision Revisions of this file
ESKF.cpp Show annotated file Show diff for this revision Revisions of this file
ESKF.h Show annotated file Show diff for this revision Revisions of this file
Eigen.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.h Show annotated file Show diff for this revision Revisions of this file
comms.h Show annotated file Show diff for this revision Revisions of this file
kinematics.cpp Show annotated file Show diff for this revision Revisions of this file
kinematics.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
ntp.cpp Show annotated file Show diff for this revision Revisions of this file
ntp.h Show annotated file Show diff for this revision Revisions of this file
odrive.cpp Show annotated file Show diff for this revision Revisions of this file
servoAxis.cpp Show annotated file Show diff for this revision Revisions of this file
servoAxis.h Show annotated file Show diff for this revision Revisions of this file
syncTime.cpp Show annotated file Show diff for this revision Revisions of this file
syncTime.h Show annotated file Show diff for this revision Revisions of this file
diff -r 778bc352c47f -r 01e1e68309ae Axis.cpp
--- a/Axis.cpp	Sun Oct 07 19:40:12 2018 +0000
+++ b/Axis.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -3,12 +3,13 @@
 
 extern BufferedSerial buffered_pc;
 
-Axis::Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName indentity)
+Axis::Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName identity)
 {
     odrive = od;
     axNum = ax;
     homeSwitch_ = homeSwitch;
-    switch(indentity) {
+    name = identity;
+    switch(identity) {
         case AX_A:
             rotation_offset = calibration_.Aoffset;
             break;
@@ -55,6 +56,39 @@
     currentSetVel = 0;
 }
 
+int Axis::readState(){
+        std::stringstream ss;
+    ss<< "r axis" << axNum << ".current_state" << '\n';
+    //buffered_pc.printf(ss.str().c_str());
+    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+    return odrive->readInt();
+    }
+
+void Axis::findIndex()
+{
+
+
+    int requested_state;
+    requested_state = ODrive::AXIS_STATE_ENCODER_INDEX_SEARCH;
+    runState(requested_state);
+    int foundIndex = 0;
+    while(!foundIndex) {
+        Thread::wait(10);
+        int state = readState();
+        if( state == ODrive::AXIS_STATE_IDLE) foundIndex = 1;
+    }
+    int requestedState;
+    requestedState = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL;
+    runState(requestedState);
+}
+
+void Axis::idle()
+{
+    int requested_state;
+    requested_state = ODrive::AXIS_STATE_IDLE;
+    runState(requested_state);
+}
+
 void  Axis::goAngle(float angle)
 {
     odrive->SetPosition(axNum,homeOffset - angle*pulse_per_rad - rotation_offset*pulse_per_rad);
@@ -70,10 +104,101 @@
     odrive->run_state(axNum, requestedState, false); // don't wait
 }
 
+int Axis::setParams(float stepsPerSec, float vel_gain, float encoder_bandwidth, float pos_gain, float vel_int)
+{
+    int error = 0;
+    error += writeParam(".controller.config.vel_limit", stepsPerSec);
+    error +=writeParam(".controller.config.vel_gain", vel_gain);
+    error +=writeParam(".controller.config.pos_gain", pos_gain);
+    error +=writeParam(".encoder.config.bandwidth", encoder_bandwidth);
+    error +=writeParam(".controller.config.vel_integrator_gain", vel_int);
+    return error;
+
+//    std::stringstream ss;
+//    ss<< "w axis" << axNum << ".controller.config.vel_limit " << stepsPerSec << '\n';
+//    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+//    ss.clear();
+//    ss<< "w axis" << axNum << ".controller.config.vel_gain " << vel_gain << '\n';
+//    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+//    ss.clear();
+//    ss<< "w axis" << axNum << ".controller.config.pos_gain " << encoder_bandwidth << '\n';
+//    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+//    ss.clear();
+//    ss<< "w axis" << axNum << ".encoder.config.bandwidth " << pos_gain << '\n';
+//    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+//    ss.clear();
+//    ss<< "w axis" << axNum << ".controller.config.vel_integrator_gain " << vel_int << '\n';
+//    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+
+}
+
 void Axis::setMaxVel(float stepsPerSec)
 {
     std::stringstream ss;
     ss<< "w axis" << axNum << ".controller.config.vel_limit " << stepsPerSec << '\n';
     buffered_pc.printf(ss.str().c_str());
     odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+}
+float Axis::readParam(string in)
+{
+    Thread::wait(10);
+    std::stringstream ss;
+    ss <<"r axis" << axNum << in << '\n';
+    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+    return odrive->readFloat();
+}
+float Axis::readBattery()
+{
+    std::stringstream ss;
+    ss <<"r vbus_voltage"<< '\n';
+    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+    return odrive->readFloat();
+}
+
+
+float Axis::writeParam(string in, float val)
+{
+    Thread::wait(10);
+    std::stringstream ss;
+    ss<< "w axis" << axNum << in<< " " << val << '\n';
+    odrive->serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
+    float read = readParam(in);
+    if(read == val) return 0;
+    else return 1;
+}
+
+int Axis::test()
+{
+    int wait = 1;
+    int waitTime = 10;
+    if(wait) Thread::wait(waitTime);
+    int error = 0;
+    float battery = readBattery();
+    if(wait) Thread::wait(waitTime);
+    battery = readBattery();
+    if(wait) Thread::wait(waitTime);
+    //buffered_pc.printf("Battery: %f \r\n",battery);
+    if (battery <1.0) error++;
+    float param = readParam(".controller.config.vel_limit");
+    if(wait) Thread::wait(waitTime);
+    //buffered_pc.printf("param: %f\r\n",param);
+    writeParam(".controller.config.vel_limit", param+1.0);
+    if(wait) Thread::wait(waitTime);
+    float param2 = readParam(".controller.config.vel_limit");
+    if(wait) Thread::wait(waitTime);
+    //buffered_pc.printf("param2: %f\r\n",param2);
+    if(param2 != param + 1) error ++;
+    writeParam(".controller.config.vel_limit", param);
+    if(wait) Thread::wait(waitTime);
+    float param3 = readParam(".controller.config.vel_limit");
+    if(wait) Thread::wait(waitTime);
+    //buffered_pc.printf("param2: %f\r\n",param3);
+    if(param3 != param) error ++;
+    if (error!=0) {
+        buffered_pc.printf("There have been %d errors on axis %d\r\n",error, name);
+    } else {
+        //buffered_pc.printf("there were no errors on axis %d  :) \r\n", name);
+    }
+    return error;
+
 }
\ No newline at end of file
diff -r 778bc352c47f -r 01e1e68309ae Axis.h
--- a/Axis.h	Sun Oct 07 19:40:12 2018 +0000
+++ b/Axis.h	Mon Oct 15 18:30:20 2018 +0000
@@ -18,14 +18,23 @@
 {
 public:
 
-    Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName indentity);
+    Axis(ODrive* od, int ax, DigitalIn* homeSwitch, calVals calibration_,axisName identity);
     void   homeAxis();
+    void    findIndex();
     void   goAngle(float angle);
     void  goAngleSpeed(float angle, float speed);
     void runState(int requestedState);
+    int readState();
     DigitalIn* homeSwitch_;
     void setMaxVel(float stepsPerSec);
+    int setParams(float stepsPerSec, float vel_gain, float encoder_bandwidth, float pos_gain, float vel_int);
     ODrive* odrive;
+    void idle();
+    int test();
+    float readParam(string in);
+    float readBattery();
+    float writeParam(string in, float val);
+    axisName name;
 
 private:
     
diff -r 778bc352c47f -r 01e1e68309ae ESKF.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ESKF.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,211 @@
+#include <ESKF.h>
+
+
+ESKF::ESKF(Matrix<float, 19,1> initialState, float sig2_a_n_, float sig2_omega_n_,float sig2_a_w_, float sig2_omega_w_){
+    trueState = initialState;
+    sig2_a_n = sig2_a_n_;
+    sig2_omega_n = sig2_omega_n_;
+    sig2_a_w = sig2_a_w_;
+    sig2_omega_w = sig2_omega_w_;
+
+    //Build F_i,
+    F_i.setZero();
+    F_i.block(3,0,12,12).setIdentity();
+}
+
+ESKF::ESKF(){
+
+}
+
+
+Matrix<float, 19,1> ESKF::makeState(Vector3f p,Vector3f v, Quaternionf q, Vector3f a_b, Vector3f omega_b,Vector3f g ){
+
+}
+
+void ESKF::updateStateIMU(Vector3f a, Vector3f omega, float delta_t){
+
+
+}
+
+Matrix<float, 3,3> ESKF::getRotationMatrixFromState(Matrix<float, 19,1> state){
+    Matrix<float,4,1> mat = state.block(6,0,4,1);
+    Quaternionf quat(mat);
+    return quat.matrix();
+}
+
+Matrix<float,3,3> ESKF::getSkew(Vector3f in){
+   Matrix<float,3,3> out;
+   out << 0, -in(2), in(1),
+           in(2), 0, -in(0),
+           -in(1), in(0), 0;
+   return out;
+}
+
+Matrix<float,3,3> ESKF::AngAxToMat(Vector3f in){
+    float angle = in.norm();
+    Vector3f axis = in.normalized();
+    if(angle == 0) axis = Vector3f(1,0,0);
+
+
+    AngleAxisf angAx(angle,axis);
+    return angAx.toRotationMatrix();
+}
+
+void ESKF::predictionUpdate(Vector3f a, Vector3f omega, float delta_t){
+    // build F_x
+    Matrix<float, 3,3> I3 , I3dt;
+    F_x = F_x.Zero(18,18);
+    //page 59
+    I3 = I3.Identity();
+    I3dt = delta_t * I3;
+    F_x.block(0,0,3,3) = I3;
+    F_x.block(3,3,3,3) = I3;
+    F_x.block(9,9,3,3) = I3;
+    F_x.block(12,12,3,3) = I3;
+    F_x.block(15,15,3,3) = I3;
+    F_x.block(0,3,3,3) = I3dt;
+    F_x.block(3,15,3,3) = I3dt;
+    F_x.block(6,12,3,3) = -I3dt;
+
+    Matrix<float, 3,3> rotation;
+    rotation = getRotationMatrixFromState(nominalState);
+    F_x.block(3,9,3,3) = -rotation*delta_t;
+
+    // for the 2nd row and 3rd column
+    F_x.block(3,6,3,3) = - rotation * getSkew(a - nominalState.block(9,0,3,1)) * delta_t;
+
+    // for the 3rd row 3rd column
+    F_x.block(6,6,3,3) = AngAxToMat((omega - nominalState.block(12,0,3,1))*delta_t).transpose();
+
+
+    // build Q_i, this is only a diagonal matrix augmented by a scalar, so could be more efficient to for loop the relevant entries.
+
+    Q_i.setZero();
+    Q_i.block(0,0,3,3) =  delta_t * delta_t * sig2_a_n  * I3 ;
+    Q_i.block(3,3,3,3) = I3 * sig2_omega_n * delta_t * delta_t;
+    Q_i.block(6,6,3,3) = I3 * sig2_a_w * delta_t;
+    Q_i.block(9,9,3,3) = I3 * sig2_omega_w * delta_t;
+
+    //probably unnecessary copying here. Need to check if things are done inplace or otherwise. //.eval should fix this issue
+     P = (F_x*P*F_x.transpose() + F_i*Q_i*F_i.transpose()).eval();
+
+
+    //this line is apparently not needed, according to the document. // I suspect it only meant in the first iteration?????
+    //Matrix<float,18,18> errorState_new = F_x * errorState;
+    //errorState = errorState_new;
+
+
+}
+
+Matrix<float,19,1> ESKF::measurementFunc(Matrix<float,19,1> in){
+    Matrix<float,19,1> func;
+    func << 1,1,1
+            ,0,0,0
+            ,1,1,1,1
+            ,0,0,0
+            ,0,0,0
+            ,0,0,0;
+    return (in.array()*func.array()).matrix();
+}
+
+void ESKF::composeTrueState(){
+
+}
+
+
+// this function puts the errorstate into the nominal state. as per page 62
+void ESKF::injectObservedError(){
+    Matrix<float,19,1> newState;
+    // compose position
+    newState.block(0,0,3,1) = nominalState.block(0,0,3,1) + errorState.block(0,0,3,1);
+    // compose Velocity
+    newState.block(3,0,3,1) = nominalState.block(3,0,3,1) + errorState.block(3,0,3,1);
+
+    // compose Quaternion - probably this can be done in less lines.
+    Matrix<float,3,1>  angAxMat = errorState.block(6,0,3,1);
+    AngleAxisf AngAx(angAxMat.norm(),angAxMat.normalized());
+    Quaternionf qError(AngAx);
+    Matrix<float,4,1> qMat =  nominalState.block(6,0,4,1);
+    Quaternionf qNom(qMat);
+    newState.block(6,0,4,1) = (qNom*qError).coeffs();
+
+    //compose accelerometer drift
+    newState.block(10,0,3,1) = nominalState.block(10,0,3,1) + errorState.block(9,0,3,1);
+
+    //compose gyro drift.
+    newState.block(13,0,3,1) = nominalState.block(13,0,3,1) + errorState.block(12,0,3,1);
+
+    //compose gravity. (I don't think it changes anything.)
+    newState.block(16,0,3,1) = nominalState.block(16,0,3,1) + errorState.block(15,0,3,1);
+
+    //update the nominal state (I am doing a copy to be sure there is no aliasing problems etc)
+    nominalState = newState;
+}
+
+void ESKF::resetError(){
+    // set the errorState to zero
+    errorState.Zero();
+
+    // set up G matrix, can be simply an identity or with a more compicated term for the rotation section.
+    G.setIdentity();
+    Matrix<float,3,3> rotCorrection;
+    rotCorrection = - getSkew(0.5*errorState.block(6,0,3,1));
+    G.block(6,6,3,3) = (G.block(6,6,3,3) + rotCorrection).eval();
+    P = (G * P * G.transpose()).eval();
+
+}
+
+
+// this function is called when you have a reference to correct the error state, in this case a mocap system.
+void ESKF::observeErrorState(Vector3f pos, Quaternionf rot){
+    Matrix<float,19,1> y;
+    y.Zero();
+    y.block(0,0,3,1) = pos;
+    y.block(6,0,4,1) <<rot.x() , rot.y() , rot.z() , rot.w();
+
+    // setup X_dx, essensially an identity, with some quaternion stuff in the middle. Optimise by initilising everything elsewhere.
+    X_dx.Zero();
+    Matrix<float, 6,6> I6;
+    I6 = I6.Identity();
+    Matrix<float,9,9> I9;
+    I9 = I9.Identity();
+    X_dx.block(0,0,6,6) = I6;
+    X_dx.block(10,9,9,9) = I9;
+    Matrix<float,4,1> q(nominalState.block(6,0,4,1)); // getting quaternion, though in a mat, so we can divide by 2.
+    q = q/2;
+    X_dx.block(6,6,4,3) <<   -q.x() , -q.y() , -q.z(),
+                              q.w() , -q.z() ,  q.y(),
+                              q.z() ,  q.w() , -q.x(),
+                             -q.y() ,  q.x() ,  q.w();
+
+    // then set up H_x, though this is not told to us directly, it is described as:
+    /*"Here, Hx , ∂h∂xt|x
+    is the standard Jacobian of h() with respect to its own argument (i.e.,
+    the Jacobian one would use in a regular EKF). This first part of the chain rule depends on
+    the measurement function of the particular sensor used, and is not presented here.*/
+
+    //I believe that if I am only providing position and a quaternion then the position part will be an identity, the quaternion part will be identity?
+
+    H_x = H_x.Identity();
+
+    //compose the two halves of the hessian
+    H = H_x*X_dx;
+    Matrix<float,19,19> V; //  the covariance of the measurement function.
+    V.Zero();
+    K = P * H.transpose() * (H*P*H.transpose() + V).inverse();
+
+    Matrix<float,18,1> d_x_hat;
+    composeTrueState();
+    d_x_hat = K *(y - measurementFunc(trueState));
+    Matrix<float,18,18> I18;
+    I18 = I18.Identity();
+    P = ((I18 - K*H)*P).eval();
+
+    injectObservedError();
+
+    resetError();
+
+
+
+
+}
diff -r 778bc352c47f -r 01e1e68309ae ESKF.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ESKF.h	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,78 @@
+#include <Core.h>
+#include <Geometry.h>
+#include <iostream>
+
+
+#define GRAVITY     9.812  // London g value.
+#define SUPPORT_STDIOSTREAM
+
+using namespace Eigen;
+using namespace std;
+
+//the main ESKF class
+class ESKF{
+public:
+    // takes as input the  variance of the acceleration and gyro, where _n is the measurement noise, and _w is the pertibations of the system.
+    ESKF(Matrix<float, 19,1> initialState, float sig2_a_n_, float sig2_omega_n_,float sig2_a_w_, float sig2_omega_w_);
+    ESKF();
+    // concatonates  relevant vectors to one large vector.
+    Matrix<float, 19,1> makeState(Vector3f p,Vector3f v, Quaternionf q, Vector3f a_b, Vector3f omega_b,Vector3f g );
+
+    // Called when there is a new measurment from the IMU.
+    void updateStateIMU(Vector3f a, Vector3f omega, float delta_t);
+
+    // Called when there is a new measurment from an absolute position reference (such as Motion Capture, GPS, map matching etc )
+    void observeErrorState(Vector3f pos, Quaternionf rot);
+//private:
+
+    Matrix<float, 3,3> getRotationMatrixFromState(Matrix<float, 19,1> state);
+    Matrix<float,3,3> getSkew(Vector3f in);
+    Matrix<float,3,3> AngAxToMat(Vector3f in);
+    Matrix<float,19,1> measurementFunc(Matrix<float,19,1> in);
+    void composeTrueState();
+    void injectObservedError();
+    void resetError();
+
+    void predictionUpdate(Vector3f a, Vector3f omega, float delta_t);
+
+    // states
+    Matrix<float, 19,1> trueState;
+    Matrix<float, 18,1> errorState;
+    Matrix<float, 19,1> nominalState;
+
+
+    //covarience matrices as defined on page 59
+    Matrix<float, 3,3> V_i;
+    Matrix<float, 3,3> PHI_i;
+    Matrix<float, 3,3> A_i;
+    Matrix<float, 3,3> OMEGA_i;
+    float sig2_a_n;
+    float sig2_a_w;
+    float sig2_omega_n;
+    float sig2_omega_w;
+
+
+    //jacobians of f() as defined on page 59// not sure if should be 19 in size, the quaternion seems to be a rotation matrix here.
+    Matrix<float, 18,18> F_x;
+    Matrix<float, 18,12> F_i;
+    //covariances matrix of the perturbation impulses.
+    Matrix <float, 12,12> Q_i;
+
+    // the P matrix
+    Matrix<float, 18,18> P;
+
+    // the K matrix
+    Matrix<float, 18,19> K;
+
+
+    //Jacobian of the true state with respect to the error state. Page 61.
+    Matrix<float, 19,18> H;
+    Matrix<float, 19, 19> H_x;
+    Matrix<float, 19, 18> X_dx;
+
+    Matrix<float, 4, 3> Q_dTheta;
+
+    // Jacobian matrix defined on page 63, can have a simple implementation as an Identity, or one that has a correction term
+    Matrix<float, 18,18> G;
+
+};
diff -r 778bc352c47f -r 01e1e68309ae Eigen.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eigen.lib	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16
diff -r 778bc352c47f -r 01e1e68309ae MPU6050.h
--- a/MPU6050.h	Sun Oct 07 19:40:12 2018 +0000
+++ b/MPU6050.h	Mon Oct 15 18:30:20 2018 +0000
@@ -153,7 +153,7 @@
 int Ascale = AFS_2G;
  
 //Set up I2C, (SDA,SCL)
-I2C i2c(PB_9, PB_8);
+extern I2C i2c;
  
 DigitalOut myled(LED1);
    
@@ -175,7 +175,7 @@
 int count = 0;  // used to control display output rate
  
 // parameters for 6 DoF sensor fusion calculations
-float PI = 3.14159265358979323846f;
+//float PI = 3.14159265358979323846f;
 float GyroMeasError = PI * (60.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
 float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
 float GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
@@ -466,8 +466,8 @@
     gyro_bias[1]  /= (int32_t) packet_count;
     gyro_bias[2]  /= (int32_t) packet_count;
     
-  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
-  else {accel_bias[2] += (int32_t) accelsensitivity;}
+  if(accel_bias[1] > 0L) {accel_bias[1] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
+  else {accel_bias[1] += (int32_t) accelsensitivity;}
  
 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
   data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
@@ -526,12 +526,12 @@
   data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  
   // Push accelerometer biases to hardware registers
-//  writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]);  
-//  writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]);
-//  writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]);
-//  writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]);  
-//  writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]);
-//  writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]);
+  writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]);  
+  writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]);
+  writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]);
+  writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]);  
+  writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]);
+  writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]);
  
 // Output scaled accelerometer biases for manual subtraction in the main program
    dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 
diff -r 778bc352c47f -r 01e1e68309ae comms.h
--- a/comms.h	Sun Oct 07 19:40:12 2018 +0000
+++ b/comms.h	Mon Oct 15 18:30:20 2018 +0000
@@ -1,32 +1,39 @@
 #ifndef COMMS_H
 #define COMMS_H
 
-
+#include <stdint.h>
 struct OdriveCMD{
     float position;
     float velocity;
     float torque;
     float maxCurrent;
-    int mode;
+    int32_t mode;
 };
 struct ServoCMD{
     float angle;
     float atTime;
 };
 
+struct rosTime{
+    int32_t seconds;
+    int32_t nSeconds;
+};
+
 struct toRobot{
     OdriveCMD motor1;
     OdriveCMD motor2;
     OdriveCMD motor3;
     ServoCMD servo1;
     ServoCMD servo2;
-    long time;
-    int count;
+    rosTime time;
+    float timeOffset;
+    int32_t isPing;
+    int32_t count;
 };
 
 struct OdriveINFO{
     float speed;
-    int position;
+    int32_t position;
     float busVoltage;
     float setCurrent;
     float measuredCurrent;
@@ -53,8 +60,10 @@
     OdriveINFO motor3;
     imuINFO imu;
     gunINFO gun;
-    long time;
-    int count;
+    rosTime time;
+    rosTime pingTime;
+    int32_t isPing;
+    int32_t count;
 };
 
 
diff -r 778bc352c47f -r 01e1e68309ae kinematics.cpp
--- a/kinematics.cpp	Sun Oct 07 19:40:12 2018 +0000
+++ b/kinematics.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -109,15 +109,18 @@
 
 void Kinematics::activateMotors()
 {
-    C->setMaxVel(15001); // for safety of the linkages, override to go fast!
-    Thread::wait(100);
-    A->setMaxVel(15001);
-    Thread::wait(100);
-    B->setMaxVel(15001);
-    Thread::wait(100);
-    C->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002);
-    B->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002);
-    A->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002);
+    //int error = 0;
+    //C->setMaxVel(15001); // for safety of the linkages, override to go fast!
+
+//    Thread::wait(100);
+//        C->setSafePerams(15004, 0.00025, 4000, 250,0.001);
+//    Thread::wait(100);
+//    B->setSafePerams(15004, 0.00025, 4000, 250,0.001);
+//    Thread::wait(100);
+//    A->setSafePerams(15004, 0.00025, 4000, 250,0.001);
+//    Thread::wait(100);
+
+    //buffered_pc.printf("there were %d errors in setting perameters",error);
 
     int requested_state;
     requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL;
@@ -126,6 +129,39 @@
     C->runState(requested_state);
 }
 
+int Kinematics::setSafeParams(){
+        int error = 0;
+        error += C->setParams(15004, 0.00025, 4000, 250,0.001);
+    Thread::wait(1);
+    error +=B->setParams(15004, 0.00025, 4000, 250,0.001);
+    Thread::wait(1);
+    error +=A->setParams(15004, 0.00025, 4000, 250,0.001);
+    Thread::wait(1);
+     buffered_pc.printf("there were %d errors in setting perameters",error);
+     return error;
+    }
+    
+    int Kinematics::setFastParams(){
+        int error = 0;
+        error += C->setParams(40000, 0.00032, 4000, 300,0.001);
+    Thread::wait(1);
+    error +=B->setParams(40000, 0.00032, 4000, 300,0.001);
+    Thread::wait(1);
+    error +=A->setParams(40000, 0.00032, 4000, 300,0.001);
+    Thread::wait(1);
+     buffered_pc.printf("there were %d errors in setting perameters",error);
+     return error;
+    }
+
+void Kinematics::findIndex(){
+    Thread::wait(100);
+    A->findIndex();
+    Thread::wait(100);
+    B->findIndex();
+    Thread::wait(100);
+    C->findIndex();
+}
+
 void Kinematics::homeMotors()
 {
     Thread::wait(1000);
@@ -136,6 +172,16 @@
     C->homeAxis();
 }
 
+void Kinematics::goIdle(){
+    Thread::wait(100);
+    A->idle();
+    Thread::wait(100);
+    B->idle();
+    Thread::wait(100);
+    C->idle();
+    Thread::wait(100);
+}
+
 int Kinematics::goToPos(float x, float y, float z)
 {
     //int error = delta_calcInverse(x,y,z,a,b,c);
diff -r 778bc352c47f -r 01e1e68309ae kinematics.h
--- a/kinematics.h	Sun Oct 07 19:40:12 2018 +0000
+++ b/kinematics.h	Mon Oct 15 18:30:20 2018 +0000
@@ -29,7 +29,11 @@
     void goToPosVel(float x, float y, float z, float xv, float yv, float zv);
     void activateMotors();
     void homeMotors();
+    void findIndex();
+    void goIdle();
     void updateCalibration(calVals calibration_);
+    int setSafeParams();
+    int setFastParams();
     DeltaKinematics<float>* DK;
 
 
diff -r 778bc352c47f -r 01e1e68309ae main.cpp
--- a/main.cpp	Sun Oct 07 19:40:12 2018 +0000
+++ b/main.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -5,16 +5,30 @@
 #include <string>
 #include "calibration.h"
 #include "Axis.h"
+#include "servoAxis.h"
 #include "kinematics.h"
 #include "BufferedSerial.h"
+#include    "syncTime.h"
+#include "MPU6050.h"
+#include "ESKF.h"
+#define EXTPIN1 PB_5  //pwm spi1_mosii
+#define EXTPIN2 PB_15 // PWM spi2 mosi
+#define EXTPIN3 PB_13   //PWM spi2sclk
+#define EXTPIN4 PB_12   //
+#define EXTPIN5 PA_15   //pwm
+#define EXTPIN6 PC_7    //RX6 PWM
 DigitalOut led1(LED1);
 DigitalOut homeGND(PF_5);
 DigitalIn homeSwitchA(PA_3);
 DigitalIn homeSwitchB(PC_0);
 DigitalIn homeSwitchC(PC_3);
+DigitalOut accelPower(PA_6);
 DigitalIn trigger(PF_3);
-BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
+BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX,1024);
 calVals calibration;
+MPU6050 mpu;
+I2C i2c(PB_9, PB_8);
+
 
 
 using std::string;
@@ -22,41 +36,96 @@
 const int BROADCAST_PORT_R = 58081;
 EthernetInterface eth;
 
-
-
-
+SyncTime timeTracker(0,0);
+float batteryV = -1;
+UDPSocket socket;
+int isCon = 0;
 void transmit()
 {
-    UDPSocket socket(&eth);
+    while(isCon != 0) {
+        Thread::yield();
+    }
     string out_buffer = "very important data";
     SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
     fromRobot msg;
     msg.motor1.busVoltage = 69;
+    buffered_pc.printf("starting send loop");
     while (true) {
         msg.motor1.busVoltage += 1;
-        int ret = socket.sendto(transmit, &msg, sizeof(msg));
+        //int ret = socket.sendto(transmit, &msg, sizeof(msg));
         //printf("sendto return: %d\n", ret);
 
         Thread::wait(100);
     }
 }
-
+fromRobot pingMsg;
 void receive()
 {
+    eth.connect();
+
+    socket.open(&eth);
+    
+    socket.set_blocking(false);
+    
+
+ 
+    buffered_pc.printf("Ethernet returned %d \n\r", isCon);
+    Thread::wait(100);
+    if(isCon !=0) eth.disconnect();
+    while(isCon != 0) {
+        Thread::yield();
+    }
+    socket.open(&eth);
+    buffered_pc.printf("Ethernet is connected at %s \n\r", eth.get_ip_address());
+    Thread::wait(100);
     SocketAddress receive;
-    UDPSocket socket(&eth);
+    //UDPSocket socket(&eth);
     int bind = socket.bind(BROADCAST_PORT_R);
+    SocketAddress transmit("10.0.0.160", BROADCAST_PORT_T);
     //printf("bind return: %d", bind);
 
     char buffer[256];
+    
+    
+        buffered_pc.printf("starting receive loop");
+        rosTime now;
+        Timer debugTimer;
+        
     while (true) {
         //printf("\nWait for packet...\n");
         int n = socket.recvfrom(&receive, buffer, sizeof(buffer));
+        if(n != NSAPI_ERROR_WOULD_BLOCK){
+        //debugTimer.start();
         toRobot inmsg;
         memcpy(&inmsg, buffer, n);
+        if(inmsg.isPing ==1) {
+            now = timeTracker.getTime();
+            if(now.seconds < 10000){
+                buffered_pc.printf("Hard resetting time to  %ds and %dns \n\r",inmsg.time.seconds,inmsg.time.nSeconds);
+                timeTracker.hardReset(inmsg.time.seconds,inmsg.time.nSeconds);
+            }
+
+            pingMsg.isPing = 1;
+            pingMsg.pingTime.seconds = inmsg.time.seconds;
+            pingMsg.pingTime.nSeconds = inmsg.time.nSeconds;
+            pingMsg.time.seconds = now.seconds;
+            pingMsg.time.nSeconds = now.nSeconds;
+            int ret = socket.sendto(transmit, &pingMsg, sizeof(pingMsg));
+            //debugTimer.stop();
+            
+            //buffered_pc.printf("sectionTime %d \r\n", debugTimer.read_high_resolution_us());
+            
+            //buffered_pc.printf("current time is %ds and %dns \r\n", now.seconds, now.nSeconds);
+        }
+            if(inmsg.timeOffset !=0.0f){
+                //buffered_pc.printf("Applying offset %fs \n\r",inmsg.timeOffset); 
+                timeTracker.updateTime(inmsg.timeOffset);            
+            }
         //buffer[n] = '\0';
-        buffered_pc.printf("Count:%i, Motor1Vel:%f, Motor1Tor: %f \r\n",inmsg.count, inmsg.motor1.velocity,inmsg.motor1.torque);
-
+        //buffered_pc.printf("Count:%i, Motor1Vel:%f, Motor1Tor: %f \r\n",inmsg.count, inmsg.motor1.velocity,inmsg.motor1.torque);
+        }
+        //debugTimer.reset();
+        Thread::yield();
         //Thread::wait(1);
     }
 }
@@ -68,53 +137,219 @@
 
 void runOdrive()
 {
+    //start servos on endEffector
+    ServoAxis pitch(EXTPIN1,30, -30, 1500, 1200.0/120.0);
+    ServoAxis yaw(EXTPIN2,30, -30, 1500, 1200.0/120.0);
+//    while(1) {
+//        pitch.setAngle(30);
+//        yaw.setAngle(30);
+//        Thread::wait(300);
+//        pitch.setAngle(-30);
+//        yaw.setAngle(-30);
+//        Thread::wait(300);
+//        }
+        
+    
     BufferedSerial ODSerial0(PC_12,PD_2);
-    ODSerial0.baud(115200);
+    ODSerial0.baud(921600);
     BufferedSerial ODSerial1(PD_5,PD_6);
-    ODSerial1.baud(115200);
+    ODSerial1.baud(921600);
     ODrive OD0(ODSerial0);
     ODrive OD1(ODSerial1);
-    //Axis A(&OD1, 1, &homeSwitchA,calibration,AX_A);
-//    Axis B(&OD1, 0, &homeSwitchB,calibration,AX_B);
-//    Axis C(&OD0, 0, &homeSwitchC,calibration,AX_C);
-//    Kinematics kin(&A, &B, &C, calibration); // the Kinematics class contains everything
-//    kin.activateMotors();
-//    kin.homeMotors();
-//    //int error = kin.goToPos(0,0,-10);
-//    kin.goToAngles(pi/4,pi/4,pi/4);
-//    Thread::wait(1000);
-//    float inc = pi /500;
-//    float i = 0;
-//    float k = -100;
-//    int up = 0;
+    Axis A(&OD1, 1, &homeSwitchA,calibration,AX_A);
+    Axis B(&OD1, 0, &homeSwitchB,calibration,AX_B);
+    Axis C(&OD0, 0, &homeSwitchC,calibration,AX_C);
+    int error = 0;
+    error += A.test();
+    error +=B.test();
+    error +=C.test();
+    buffered_pc.printf("there were %d errors in the read/write\r\n",error);
+    Kinematics kin(&A, &B, &C, calibration); // the Kinematics class contains everything
+    buffered_pc.printf("setting motors to idle\r\n");
+    kin.goIdle();
+    kin.goIdle();
+    error += kin.setSafeParams();
+    while(error) {}
+//    kin.goIdle();//for some reason we need to do it multiple times!
+    while(*A.homeSwitch_||*B.homeSwitch_ || *C.homeSwitch_) {
+       Thread::wait(5); 
+       batteryV = OD1.readBattery();
+       } // wait till user
+    buffered_pc.printf("finding index\r\n");
+    kin.findIndex();
+    buffered_pc.printf("activating motors\r\n");
+    kin.activateMotors();
+    buffered_pc.printf("homing motors\r\n");
+    kin.homeMotors();
+    //int error = kin.goToPos(0,0,-10);
+
+    kin.goToAngles(pi/4,pi/4,pi/4);
+    Thread::wait(500);
+    error += kin.setFastParams();
+    while(error) {}
+    Thread::wait(1000);
+
+    Thread::wait(100);
+    float inc = pi /150;
+    float i = 0;
+    float k = -100;
+    int up = 0;
+    float min = 80;
+    float max =220;
+    float mid = min+max;
+    mid/= 2;
+    float span = mid - min;
+    float radius = 40;
+    int loopCounter = 0;
     while(true) {
- //       int count = 0;
-//        for(int j = 0 ; j < 100; j++) {
-//
-//            if(trigger == false) {
-//                count++;
-//            }
-//        }
-//        if(count>90) {
-//            i += inc;
-//            if(up == 1)k+=0.05;
-//            else k-=0.05;
-//            if(k >= -80) up = 0;
-//            if(k <= -180) up = 1;
-//            //int error = kin.goToPos(40.0*sin(i),40.0*cos(i),k);
-//            //int error = kin.goToPos(0,0,k);
-//            if(i > 2*pi) i = 0;
-            Thread::wait(10);
-            buffered_pc.printf("bus voltage %f\r\n",OD1.readBattery());
-            //buffered_pc.printf("k= %f\r\n",k);
-        //}
+        loopCounter++;
+        int count = 0;
+        for(int j = 0 ; j < 100; j++) {
+
+            if(trigger == false) {
+                count++;
+            }
+        }
+        if(count>90) {
+            i += inc;
+            if(up == 1)k+=0.2;
+            else k-=0.2;
+            if(k >= -min) up = 0;
+            if(k <= -max) up = 1;
+            radius =0.6* (span - abs(abs(k) - mid));
+            //buffered_pc.printf("x,y,z = %f %f %f",radius*sin(i),radius*cos(i),k);
+            int error = kin.goToPos(40.0*sin(i),40.0*cos(i),k);
+            pitch.setAngle(25.0*sin(i));
+            yaw.setAngle(25.0*cos(i));
+            //int error = kin.goToPos(0,0,k);
+            if(i > 2*pi) i = 0;
+
+//            buffered_pc.printf("bus voltage %f\r\n",OD1.readBattery());
+//            buffered_pc.printf("k= %f\r\n",k);
+        }
+        Thread::wait(1);
+        if(loopCounter % 2000 == 0) {
+            batteryV = OD1.readBattery();
+        }
     }
 
 }
 
+void accelThread()
+{
+    //mpu.initMPU6050();
+    accelPower = 1;
+    uint8_t whoami = mpu.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+    buffered_pc.printf("I AM 0x%x\n\r", whoami);
+    buffered_pc.printf("I SHOULD BE 0x68\n\r");
+    mpu.resetMPU6050(); // Reset registers to default in preparation for device calibration
+    mpu.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+    mpu.initMPU6050();
+    buffered_pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+    Timer t;
+    t.start();
+    float sum = 0;
+    uint32_t sumCount = 0;
+    while(1) {
+        Thread::wait(1);
+
+        if(mpu.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
+            mpu.readAccelData(accelCount);  // Read the x/y/z adc values
+            mpu.getAres();
+
+            // Now we'll calculate the accleration value into actual g's
+            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+            ay = (float)accelCount[1]*aRes - accelBias[1];
+            az = (float)accelCount[2]*aRes - accelBias[2];
+
+            mpu.readGyroData(gyroCount);  // Read the x/y/z adc values
+            mpu.getGres();
+
+            // Calculate the gyro value into actual degrees per second
+            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
+            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+
+            tempCount = mpu.readTempData();  // Read the x/y/z adc values
+            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+
+//            buffered_pc.printf("ax = %f", 1000*ax);
+//            buffered_pc.printf(" ay = %f", 1000*ay);
+//            buffered_pc.printf(" az = %f  mg\n\r", 1000*az);
+            Now = t.read_us();
+            deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+            lastUpdate = Now;
+
+            sum += deltat;
+            sumCount++;
+
+            if(lastUpdate - firstUpdate > 10000000.0f) {
+                beta = 0.04;  // decrease filter gain after stabilized
+                zeta = 0.015; // increasey bias drift gain after stabilized
+            }
+
+            // Pass gyro rate as rad/s
+            mpu.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+
+            // Serial print and/or display at 0.5 s rate independent of data rates
+            //delt_t = t.read_ms() - count;
+
+            mpu.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+
+            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]  );
+            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+            pitch *= 180.0f / PI;
+            yaw   *= 180.0f / PI;
+            roll  *= 180.0f / PI;
+//            roll += 90;
+//            if(roll > 180) roll = -(roll - 180);
+//            if(roll < -180)roll = -(roll + 180);
+            buffered_pc.printf("roll= %f   pitch = %f     yaw = %f  \n\r", roll,pitch,yaw);
+
+
+
+        }
+
+    }
+}
+
+void batteryThread()
+{
+    while(1) {
+        Thread::wait(5000);
+        buffered_pc.printf("batteryVoltage = %f\n\r", batteryV);
+    }
+
+
+}
+
+
+
 
 int main()
 {
+    
+    // testing eigen run time. 
+    
+    ESKF ourESKF;
+    Vector3f acc, gyro,pos;
+    Quaternionf quat(0.25,0.25,0.25,0.25);
+    acc = Vector3f::Random();
+    gyro = Vector3f::Random();
+    pos = Vector3f::Random();
+    Timer eigenTimer;
+    eigenTimer.reset();
+    eigenTimer.start();
+    for(int i = 0; i < 1000; i ++){
+        ourESKF.predictionUpdate(acc,gyro, 0.001);
+    }
+    for(int i = 0; i < 100; i ++){
+        ourESKF.observeErrorState(pos,quat);
+    }
+    eigenTimer.stop();
+    buffered_pc.printf("the eigen took %f seconds",eigenTimer.read());
+
 
     calibration.e = 58.095;
     calibration.f = 60.722;
@@ -126,9 +361,13 @@
     calibration.gearRatio = 89.0/24.0;
     buffered_pc.baud(115200);
     buffered_pc.printf("hello\r\n");
+    i2c.frequency(400000);
     Thread transmitter;
     Thread receiver;
     Thread odriveThread;
+    Thread accel;
+    Thread timeUpdate;
+    Thread printBattery;
 
 
     //set switches up
@@ -137,14 +376,18 @@
     homeSwitchB.mode(PullUp);
     homeSwitchC.mode(PullUp);
     trigger.mode(PullUp);
-    //eth.connect();
+
+
 
     //buffered_pc.printf("Controller IP Address is %s\r\n", eth.get_ip_address());
     Thread::wait(5000);
 
-    //transmitter.start(transmit);
-    //receiver.start(receive);
+    transmitter.start(transmit);
+    receiver.start(receive);
+    //timeUpdate.start(timeUpdateLoop);
     odriveThread.start(runOdrive);
+    accel.start(accelThread);
+    printBattery.start(batteryThread);
 
     while (true) {
         //led1 = !led1;
diff -r 778bc352c47f -r 01e1e68309ae ntp.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ntp.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,63 @@
+#include "ntp.h"
+#include "mbed.h"
+
+//this code has been taken from https://github.com/ARMmbed/ntp-client
+
+NTPClient::NTPClient(NetworkInterface *iface)
+: iface(iface), nist_server_address((char *)NTP_DEFULT_NIST_SERVER_ADDRESS), nist_server_port(NTP_DEFULT_NIST_SERVER_PORT) {
+}
+
+void NTPClient::set_server(char* server, int port){
+    nist_server_address = server;
+    nist_server_port = port;
+}
+
+time_t NTPClient::get_timestamp(int timeout) {
+    const time_t TIME1970 = (time_t)2208988800UL;
+    int ntp_send_values[12] = {0};
+    int ntp_recv_values[12] = {0};
+
+    SocketAddress nist;
+    int ret_gethostbyname = iface->gethostbyname(nist_server_address, &nist);
+
+    if (ret_gethostbyname < 0) {
+        // Network error on DNS lookup
+        return ret_gethostbyname;
+    }
+
+    nist.set_port(nist_server_port);
+
+    memset(ntp_send_values, 0x00, sizeof(ntp_send_values));
+    ntp_send_values[0] = '\x1b';
+
+    memset(ntp_recv_values, 0x00, sizeof(ntp_recv_values));
+
+    UDPSocket sock;
+    sock.open(iface);
+    sock.set_timeout(timeout);
+
+    sock.sendto(nist, (void*)ntp_send_values, sizeof(ntp_send_values));
+
+    SocketAddress source;
+    const int n = sock.recvfrom(&source, (void*)ntp_recv_values, sizeof(ntp_recv_values));
+
+    if (n > 10) {
+        return ntohl(ntp_recv_values[10]) - TIME1970;
+    } else {
+        if (n < 0) {
+            // Network error
+            return n;
+        } else {
+            // No or partial data returned
+            return -1;
+        }
+    }
+}
+
+uint32_t NTPClient::ntohl(uint32_t x) {
+    uint32_t ret = (x & 0xff) << 24;
+    ret |= (x & 0xff00) << 8;
+    ret |= (x & 0xff0000UL) >> 8;
+    ret |= (x & 0xff000000UL) >> 24;
+    return ret;
+}
\ No newline at end of file
diff -r 778bc352c47f -r 01e1e68309ae ntp.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ntp.h	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,18 @@
+#include "mbed.h"
+
+#define NTP_DEFULT_NIST_SERVER_ADDRESS "2.pool.ntp.org"
+#define NTP_DEFULT_NIST_SERVER_PORT 123
+
+class NTPClient {
+    public:
+        NTPClient(NetworkInterface *iface);
+        void set_server(char* server, int port);
+        time_t get_timestamp(int timeout = 15000);
+
+    private:
+        NetworkInterface *iface;
+        char* nist_server_address;
+        int nist_server_port;
+
+        uint32_t ntohl(uint32_t num);
+};
\ No newline at end of file
diff -r 778bc352c47f -r 01e1e68309ae odrive.cpp
--- a/odrive.cpp	Sun Oct 07 19:40:12 2018 +0000
+++ b/odrive.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -84,13 +84,13 @@
         str += c;
     }
     timer.stop();
-    buffered_pc.printf(str.c_str());
+    //buffered_pc.printf(str.c_str());
     return str;
 }
 
 float ODrive::readBattery(){
         std::stringstream ss;
-    ss   <<"r vbus_voltage\n" << '\n';
+   ss   <<"r vbus_voltage"<< '\n';
     serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
     return readFloat();
     
diff -r 778bc352c47f -r 01e1e68309ae servoAxis.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servoAxis.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,20 @@
+#include "servoAxis.h" 
+
+ServoAxis::ServoAxis(PinName pin,float maxAngle_, float minAngle_, float middleUs_, float usPerDeg_){
+    maxAngle = maxAngle_;
+    minAngle = minAngle_;
+    middleUs = middleUs_;
+    usPerDeg = usPerDeg_;
+    pwm = new PwmOut(pin);
+    pwm->period_ms(4);
+    pwm->pulsewidth_us(middleUs);
+    
+    }
+    
+    
+    void ServoAxis::setAngle(float angle){
+        if(angle > maxAngle) angle = maxAngle;
+        if(angle < minAngle) angle = minAngle;
+        pwm->pulsewidth_us(middleUs + angle * usPerDeg);
+        
+        }
\ No newline at end of file
diff -r 778bc352c47f -r 01e1e68309ae servoAxis.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servoAxis.h	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,28 @@
+#ifndef SERVO_AXIS_H
+#define SERVO_AXIS_H
+
+#include "mbed.h"
+
+
+class ServoAxis
+{
+public:
+
+    ServoAxis(PinName pin,float maxAngle_, float minAngle, float middleUs_, float usPerDeg_);
+    void setAngle(float angle);
+
+private:
+    PwmOut* pwm;
+    float middleUs;
+    float usPerDeg;
+
+    float maxAngle; //roughtly
+    float minAngle;
+
+};
+
+
+
+
+
+#endif // SERVO_AXIS_H
\ No newline at end of file
diff -r 778bc352c47f -r 01e1e68309ae syncTime.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/syncTime.cpp	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,64 @@
+#include "syncTime.h"
+extern BufferedSerial buffered_pc;
+SyncTime::SyncTime( int seconds, int nSeconds ){
+    refTime.seconds = seconds;
+    refTime.nSeconds = nSeconds;
+    sinceRefTimer.reset();
+    sinceRefTimer.start();
+    
+}
+    const int million = 1000000;
+    const int billion = 1000000000;
+    
+rosTime SyncTime::getTime(){
+    rosTime ret;
+    uint64_t uSecs = sinceRefTimer.read_high_resolution_us();
+    ret.seconds = (int)(uSecs/million);
+    ret.nSeconds = (int)((uSecs% million)*1000);
+    ret.seconds += refTime.seconds;
+    ret.nSeconds += refTime.nSeconds;
+        if(ret.nSeconds >= (int)1e9){
+        ret.seconds ++;
+        ret.nSeconds -= (int)1e9;
+    }
+    if(ret.nSeconds < 0){
+        ret.seconds --;
+        ret.nSeconds += (int)1e9;
+        }
+    return ret;
+}
+
+void SyncTime::resetOffsetCounter(){
+    rosTime update = getTime();
+    refTime = update;
+    sinceRefTimer.reset();   
+    //buffered_pc.printf("resetting counter %dus\r\n", sinceRefTimer.read_high_resolution_us());
+    
+}
+
+void SyncTime::hardReset(int seconds, int nSeconds){
+        refTime.seconds = seconds;
+    refTime.nSeconds = nSeconds;
+    sinceRefTimer.reset();
+    sinceRefTimer.start();
+    }
+    
+
+void SyncTime::updateTime(float correction){
+    int seconds = (int)correction;
+    int nSeconds = (correction - (float)seconds) * 1e9;
+//    buffered_pc.printf("time before %ds %dns \n\r", refTime.seconds, refTime.nSeconds);
+//    buffered_pc.printf("updating by  %ds %dns \n\r", seconds, nSeconds);
+    refTime.seconds += seconds;
+    refTime.nSeconds += nSeconds;
+    if(refTime.nSeconds >= billion){
+        refTime.seconds ++;
+        refTime.nSeconds -= billion;
+    }
+    if(refTime.nSeconds < 0){
+        refTime.seconds --;
+        refTime.nSeconds += billion;
+        }
+        //buffered_pc.printf("time after %ds %dns \n\r", refTime.seconds, refTime.nSeconds);
+        resetOffsetCounter();
+}
\ No newline at end of file
diff -r 778bc352c47f -r 01e1e68309ae syncTime.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/syncTime.h	Mon Oct 15 18:30:20 2018 +0000
@@ -0,0 +1,25 @@
+#ifndef SYNC_TIME_H
+#define SYNC_TIME_H
+
+#include "comms.h"
+#include "mbed.h"
+#include "BufferedSerial.h"
+
+class SyncTime{
+   public:
+   SyncTime( int seconds, int nSeconds);
+   rosTime getTime();
+   void updateTime(float correction);
+   void hardReset(int seconds, int nSeconds);
+   
+   private:
+   void resetOffsetCounter();
+   rosTime refTime;
+   Timer sinceRefTimer;
+    
+    
+    };
+
+
+
+#endif
\ No newline at end of file