Revision 5:01e1e68309ae, committed 2018-10-15

Comitter:

je310

Date:

Mon Oct 15 18:30:20 2018 +0000

Parent:

4:778bc352c47f

Commit message:

testing eigen;

Changed in this revision

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