       doStartup();
       break;
     case Operational:
-      amplificationBoardDataRead();
-//      currentDataRead();
-      positionSetpoint(encoder_data);
+      // amplificationBoardDataRead();
+      currentDataRead();
+      // positionSetpoint(encoder_data);
       break;
   }
   delay(100);

    //     }
    // Serial.println();

    current_data = buf[4];
    current_data <<= 8;
    current_data = current_data | buf[5];
    current_data <<= 8;
    current_data = current_data | buf[6];
    current_data <<= 8;
    current_data = current_data | buf[7];

    Serial.print("Current: ");
    Serial.println(current_data);

    return(current_data);


    unsigned int canId = CAN.getCanId();
    
    Serial.print("Current: ");
    
    //   for(int i = 0; i<len; i++)    // print the data
    //     {
