         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);
   }
 }
 

        break;

      // ID 321 message has information sent by the amplification board
      case 0x321:
        encoderDataRead(buf);

        // // load cell information is read from buf[1], buf[2] and buf[3] and converted to decimal
        loadcell_data = buf[1];

    
    //    reads CAN BUS
    CANDataRead();
    
    if (sync_flag){
      sync_flag=0;
