 //*******************
 // POSITION SETPOINT
 //*******************
-void positionSetpoint(void) 
+void positionSetpoint(double position) 
 {
   unsigned char len = 0;
   unsigned char buf[8];
 
-    Serial.println("Enviei a mensagem");
-    // Serial.println(setpoint_position,HEX);
-    // clear the string:
-    CAN.sendMsgBuf(0x601, 0, 8, setpoint_position);
-    delay(10);
+  uint32_t angulo = (position/4096)*200000;
+
+  setpoint_position[4] = angulo & 0xFF; 
+  setpoint_position[5] = (angulo >> 8) & 0xFF;
+  setpoint_position[6] = (angulo >> 16) & 0xFF;
+  setpoint_position[7] = (angulo >> 24) & 0xFF;
+
+  Serial.println("Position: ");
+  Serial.println(position);
+
+  Serial.println("Angulo: ");
+  Serial.println(angulo);
+
+  Serial.println("Enviei a mensagem");
+  // Serial.println(setpoint_position,HEX);
+  // clear the string:
+  CAN.sendMsgBuf(0x601, 0, 8, setpoint_position);
+  delay(10);
 }
 
 //***************
 // DATA READ
 //***************
-void dataRead(void)
+float dataRead()
 {
   unsigned char len = 0;
   unsigned char buf[8];

      break;
    case Operational:
      dataRead();
      positionSetpoint();
      break;
  }
  delay(100);

    // Serial.println(loadcell_data/1000, DEC);

    // delay(400);
       return(encoder_data);
  }

}
