     delay(1000);
 }
 
+//***************
+// DATA READ
+//***************
 void DataRead(void) {
-
-  //***************
-  // DATA READ
-  //***************
-
   unsigned char len = 0;
   unsigned char buf[8];
 

  setpoint_position[6] = (angulo >> 16) & 0xFF;
  setpoint_position[7] = (angulo >> 24) & 0xFF;



  switch (State) {
    case Startup:

  State = Operational;
}

//***************
// Position Setpoint
//***************
void PositionSetpoint(void) {

  unsigned char len = 0;
  unsigned char buf[8];
