         loadcell_data <<= 8;
         loadcell_data = loadcell_data | buf[3];
 
-//        Serial.print("Loadcell: ");
-//        Serial.println(loadcell_data);
-
-//
+        positionSetpoint(encoder_data);
+
+        //        Serial.print("Loadcell: ");
+        //        Serial.println(loadcell_data);
+
+        Serial.print("Encoder Position: ");
+        Serial.println(encoder_data);
+
         break;
       }
-      // return(encoder_data);
+      return(encoder_data);
+      return(loadcell_data);
   }
 }
 

    
    //    reads CAN BUS
    CANDataRead();

    
    if (sync_flag){
      sync_flag=0;

  }
}

// //******************************
// // POSITION DATA READ
// //******************************
// float encoderDataRead(unsigned char buf[])
// {
//     unsigned char encoder_data[8];

//     encoder_data = buf[4];
//     encoder_data <<= 8;
//     encoder_data = encoder_data | buf[5];

//     Serial.print("Encoder: ");
//     Serial.println(encoder_data);
// }

//****************
// CAN DATA READ
