/*
* // Arduino serial read asynch callback (doesn't clog up main loop)
* public IEnumerator AsynchronousReadFromArduino(Action<string> callback, Action fail = null, float timeout = float.PositiveInfinity){
* DateTime initialTime = DateTime.Now;
* DateTime nowTime;
* TimeSpan diff = default(TimeSpan);
* string dataString = null;
* do{
* try{
* dataString = arduinoSerialPort.ReadLine();
* }catch (TimeoutException){
* dataString = null;
* }
*
* if (dataString != null){
* callback(dataString);
* yield return null;
* }else{
* yield return new WaitForSeconds(0.05f);
* }
* nowTime = DateTime.Now;
* diff = nowTime - initialTime;
*
* }
* while (diff.Milliseconds < timeout);
* if (fail != null) {
* fail ();
* }
* yield return null;
* }
*/
void sendDriveCommand()
{
if (this.enabled)
{
//print("Sending drive command");
// Unity is in meters, so multiply by 10,000 to get 1mm as base unit
float distErrorf = distanceError_m * 1000.0f;
Int16 distError = (Int16)Math.Round(distErrorf);
if (distErrorf > COMMUNICATION_INT16_MAX)
{
distError = (Int16)(COMMUNICATION_INT16_MAX);
}
else if (distError < -COMMUNICATION_INT16_MAX)
{
distError = (Int16)(-COMMUNICATION_INT16_MAX);
}
// Convert angle and ensure it is in [-180, 180] degrees
float rotationErrorRadians = angleError_deg * Mathf.Deg2Rad;
while (rotationErrorRadians <= -Math.PI) // If we're less than -180 degrees, add a rotation so we're in -180<r<180
{
rotationErrorRadians += twoPi;
}
while (rotationErrorRadians >= Math.PI) // if we're more than +180 degrees, subtract a rotation so we're in -180<r<180
{
rotationErrorRadians -= twoPi;
}
Int16 rotationErrorInt = (Int16)(Math.Round((rotationErrorRadians * COMMUNICATION_INT16_MAX) / Math.PI));
// Good for debugging
// print ("D<" + distError + "mm\t" + ((((float)rotationErrorInt) * 180.0f) / 32767.0f).ToString () + "deg>");
// Byte stuffing, yay!
int dataStartIndex = 0;
driveMessageData [dataStartIndex + 0] = (byte)(distError >> 8);
driveMessageData [dataStartIndex + 1] = (byte)(distError);
driveMessageData [dataStartIndex + 2] = (byte)(rotationErrorInt >> 8);
driveMessageData [dataStartIndex + 3] = (byte)(rotationErrorInt);
int encodededLength = COBS.cobs_encode(ref driveMessageData, DRIVE_MESSAGE_DATA_BYTES, ref COBSEncodedMessage);
// print("Encoded message (" + encodededLength + ")" + BitConverter.ToString(COBSEncodedMessage));
byte[] finalMessage = new byte[encodededLength + 1];
Buffer.BlockCopy(COBSEncodedMessage, 0, finalMessage, 1, encodededLength);
finalMessage [0] = 0;
//print("Initial message (6)" + BitConverter.ToString(driveMessageData) + "\nFinal message: " + BitConverter.ToString(finalMessage));
if (arduinoSerialPort != null && arduinoSerialPort.IsOpen)
{
arduinoSerialPort.Write(finalMessage, 0, encodededLength + 1);
}
//arduinoSerialPort.BaseStream.Flush();
}
}