I achieved a better result using UART:
mbed to raptor:
Lidarlite V1 and Lidarlite V2
#include "mbed.h"
#include "LidarLite.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
Serial pc(USBTX,USBRX);
RawSerial com1(p28, p27); // TX, RX, TX len, RX len
LidarLite lidar(p9, p10); //sda, scl
DigitalOut V0(p29); // to switch lasers (front / back)
DigitalOut myled(LED2);
AnalogIn ain_X(p18);
AnalogIn ain_Y(p19);
AnalogIn ain_Z(p20);
AnalogIn ain_A(p15);
AnalogIn ain_B(p16);
AnalogIn ain_C(p17);
float A, B, C, X, Y, Z, forwardTilt1, forwardTilt2;
const int LENGTH = 21;
char tx_data[LENGTH];
char strOut[LENGTH];
char sWifi[LENGTH];
int sendLen, len, total, sent, checked;
int main() {
com1.baud(9600);
int distance, distance0, distance1;
while(1) {
myled = 0;
X = ain_X.read() * 3.33;
Y = ain_Y.read() * 3.33;
Z = ain_Z.read() * 3.33;
A = ain_A.read() * 3.33;
B = ain_B.read() * 3.33;
C = ain_C.read() * 3.33;
pc.printf("result: %f, %f, %f, %f, %f, %f\n", X, Y, Z, A, B, C);
forwardTilt1 = (asin((Y - (3.33 / 2.0)) / 0.333) * (180 / M_PI) + 3);
forwardTilt2 = (asin((B - (3.33 / 2.0)) / 0.333) * (180 / M_PI) + 3);
if (!(forwardTilt1 <= 90.0 && forwardTilt1 >= -90.0))
forwardTilt1 = 0.0;
if (!(forwardTilt2 <= 90.0 && forwardTilt2 >= -90.0))
forwardTilt2 = 0.0;
char buf[10];
sprintf(buf, "%5.1f %5.1f", forwardTilt1, forwardTilt2);
pc.printf("degrees: %s\n", buf);
wait(0.5);
distance0 = 0;
distance1 = 0;
V0 = 0;
while (1)
{
myled = 1;
distance = 0;
lidar.refreshRange();
distance = lidar.getRange_cm();
if (distance > 1000 || distance < 10)
{
pc.printf("%d %d \r\n", (int)V0, distance);
continue;
}
pc.printf("laser %d %d \r\n", (int)V0, distance);
wait(0.3);
if (V0 == 1)
{
distance1 = distance;
V0 = 0; //Blue V2
}
else
{
distance0 = distance;
V0 = 1; //Silver V1
}
wait(0.3);
if (distance0 != 0 && distance1 != 0)
break;
}
myled = 0;
for (int i = 0; i < LENGTH; i++)
{
tx_data[i] = 0x00;
strOut[i] = 0x00;
sWifi[i] = 0x00;
}
snprintf(strOut, LENGTH, "G%03.0f#%03.0f#%03d#%03d#", forwardTilt1 * 10, forwardTilt2 * 10, distance0, distance1);
//pc.printf("sending: %s\n", strOut);
sendLen = strlen(strOut);
if (sendLen > 0)
strncat(sWifi, strOut, LENGTH);
len = strlen(sWifi);
for (int i = len; i < LENGTH; i++)
sWifi[i] = '#';
//pc.printf("Prepared: %s\n", sWifi);
snprintf(tx_data, LENGTH, "%s", sWifi);
sendLen = strlen(sWifi);
if (sendLen < LENGTH)
{
tx_data[sendLen] = 0x0D;
sendLen = sendLen + 1;
}
else
{
tx_data[LENGTH - 1] = 0x0D;
sendLen = LENGTH;
}
int h = 0;
int b = 0;
total = 0;
bool faulty = false;
while (b != 13 || h < LENGTH && faulty == false)
{
b = (int)tx_data[h];
if (com1.writeable())
{
if (b >= 32 && b < 127 || b == 13)
{
b = com1.putc(b);
if (b == -1)
faulty = true;
else
++h;
}
}
else
{
wait(0.5);
if (total++ > 5)
{
total = 0;
faulty = true;
break;
}
}
}
wait(0.3);
sent++;
pc.printf("Sent: %s entry %d\r\n", sWifi, sent);
checked = com1.readable();
}
}
if (UART1 != null && UART1.IsOpen)
UART1.Close();
COMPort = "COM1";
UART1 = new SerialPort(COMPort, 9600);
UART1.Open();
//UART1.DiscardInBuffer();
numBytes = 0;
for (int k = 0; k < 100; k++)
{
if (UART1.ReadByte() == 71)
{
bBuffer[0] = 71;
numBytes++;
break;
}
}
Thread.Sleep(100);
b = 0;
sbQueue6.Clear();
while (b != 13)
{
if (UART1.BytesToRead > 0)
{
b = (byte)UART1.ReadByte();
if (b >= 32 && b <= 126)
{
bBuffer[numBytes] = b;
if (++numBytes >= 127)
{
bBuffer[127] = 0;
break;
}
}
}
else
break;
Thread.Sleep(140);
}
if (numBytes > 0)
{
bBuffer[numBytes] = 0;
str6 = new string(Encoding.UTF8.GetChars(bBuffer));
Debug.Print(str6);
d1 = 0.0;
if (str6.Length > 0)
{
sbQueue6.Append(str6);
success = false;
if (sbQueue6.ToString().Length >= 5)
{
success = TryEnqueue(outQueue, str6);
int k = 0;
for (int m = 0; m <= str6.Length; m++)
{
if (str6[m] == '#')
{
iBreak[k] = m + 1;
if (++k >= 4)
break;
}
}
dArray[0] = Convert.ToDouble(sbQueue6.ToString(1, iBreak[0] - 2));
dArray[1] = Convert.ToDouble(sbQueue6.ToString(iBreak[0], iBreak[1] - iBreak[0] - 1));
dArray[2] = Convert.ToDouble(sbQueue6.ToString(iBreak[1], iBreak[2] - iBreak[1] - 1));
dArray[3] = Convert.ToDouble(sbQueue6.ToString(iBreak[2], iBreak[3] - iBreak[2] - 1));
}
outQ = outQueue.Count;
if (success)
N7_Display("Data queued: " + sbQueue6.ToString(), 0, 0);
}
}
UART1.Close();