void Start()
{
m_meshfilter = GetComponent <MeshFilter>();
m_data = new LidarData[720];
m_ind = new List <int>();
m_vert = new List <Vector3>();
for (int i = 0; i < 720; i++)
{
m_ind.Add(i);
}
m_mesh = new Mesh();
m_mesh.MarkDynamic();
RplidarBinding.OnConnect(COM);
RplidarBinding.StartMotor();
m_onscan = RplidarBinding.StartScan();
if (m_onscan)
{
m_thread = new Thread(GenMesh);
m_thread.Start();
}
}
// Use this for initialization
void Start()
{
RplidarBinding.OnConnect(port);
RplidarBinding.StartMotor();
RplidarBinding.StartScan();
getAllData();
}
void OnDestroy()
{
m_thread.Abort();
RplidarBinding.EndScan();
RplidarBinding.EndMotor();
RplidarBinding.OnDisconnect();
RplidarBinding.ReleaseDrive();
m_onscan = false;
}
void GenMesh()
{
while (true)
{
int datacount = RplidarBinding.GetData(ref m_data);
if (datacount == 0)
{
Thread.Sleep(20);
}
else
{
m_datachanged = true;
}
}
}
static void Main(string[] args)
{
LidarData[] data = new LidarData[720];
String port;
while (true)
{
Console.WriteLine("input COM port:");
var comport = Console.ReadLine().ToUpper();
int p = 0;
if (comport.StartsWith("COM") && comport.Length == 4 && int.TryParse(comport.Substring(3), out p))
{
port = comport;
break;
}
}
RplidarBinding.OnConnect(port);
Console.WriteLine("on conn");
RplidarBinding.StartMotor();
Console.WriteLine("start motor");
RplidarBinding.StartScan();
Console.WriteLine("start scan");
var datalen = RplidarBinding.GetData(ref data);
for (var i = 0; i < datalen; i++)
{
Console.WriteLine($"{data[i].distant} {data[i].theta} {data[i].quality}");
}
Console.ReadLine();
Console.WriteLine("end scan");
RplidarBinding.EndScan();
Console.WriteLine("end motor");
RplidarBinding.EndMotor();
Console.ReadLine();
Console.WriteLine("disconnect");
RplidarBinding.OnDisconnect();
}
// Update is called once per frame
void Update()
{
getAllData();
if (Input.GetKeyDown(KeyCode.S))
{
print("Begin");
if (string.IsNullOrEmpty(port))
{
return;
}
RplidarBinding.OnConnect(port);
RplidarBinding.StartMotor();
RplidarBinding.StartScan();
}
if (Input.GetKeyDown(KeyCode.D))
{
print("Disconnect");
RplidarBinding.OnDisconnect();
}
}
void getAllData()
{
int count = RplidarBinding.GetData(ref data);
if (count > 0)
{
for (int i = 0; i < count; i++)
{
if ((data[i].theta > tp_angles[0] - 1) && (data[i].theta < tp_angles[0] + 1))
{
angles_list_0.Add((int)data[i].distant);
// print("data 0: " + data[i].distant);
if (angles_list_0.Count > dist_precision)
{
float avg_val = average(angles_list_0);
filtered_val[0] = (0.8 * filtered_val[0]) + (0.2 * avg_val);
// print(filtered_val[0]);
if ((filtered_val[0] > tp_distances[0] - 100) && (filtered_val[0] < tp_distances[0] + 100))
{
filter_counter[0]++;
if (filter_counter[0] > rdc_val)
{
button_clicked = 0;
print("0 at " + filtered_val[0]);
}
}
else
{
filter_counter[0] = 0;
}
angles_list_0.Clear();
}
}
if ((data[i].theta > tp_angles[1] - 4) && (data[i].theta < tp_angles[1] + 4))
{
angles_list_1.Add((int)data[i].distant);
// print("data 1: " + data[i].distant);
if (angles_list_1.Count > dist_precision)
{
float avg_val = average(angles_list_1);
filtered_val[1] = (0.8 * filtered_val[1]) + (0.2 * avg_val);
// print(filtered_val[1]);
if ((filtered_val[1] > tp_distances[1] - 200) && (filtered_val[1] < tp_distances[1] + 200))
{
filter_counter[1]++;
if (filter_counter[1] > rdc_val)
{
button_clicked = 1;
print("1 at " + filtered_val[1]);
}
}
else
{
filter_counter[1] = 0;
}
angles_list_1.Clear();
}
}
if ((data[i].theta > tp_angles[2] - tp_angle_tolerance) && (data[i].theta < tp_angles[2] + tp_angle_tolerance))
{
angles_list_2.Add((int)data[i].distant);
// print("data 2: " + data[i].distant);
if (angles_list_2.Count > dist_precision)
{
float avg_val = average(angles_list_2);
filtered_val[2] = (0.8 * filtered_val[2]) + (0.2 * avg_val);
// print(filtered_val[2]);
if ((filtered_val[2] > tp_distances[2] - tp_dist_tolerance) && (filtered_val[2] < tp_distances[2] + tp_dist_tolerance))
{
filter_counter[2]++;
if (filter_counter[2] > rdc_val + 2)
{
button_clicked = 2;
print("2 at " + filtered_val[2]);
}
}
else
{
filter_counter[2] = 0;
}
angles_list_2.Clear();
}
}
if ((data[i].theta > tp_angles[3] - 2) && (data[i].theta < tp_angles[3] + 2))
{
angles_list_3.Add((int)data[i].distant);
// print("data 2: " + data[i].distant);
if (angles_list_3.Count > dist_precision)
{
float avg_val = average(angles_list_3);
filtered_val[3] = (0.8 * filtered_val[3]) + (0.2 * avg_val);
// print(filtered_val[3]);
if ((filtered_val[3] > tp_distances[3] - tp_dist_tolerance) && (filtered_val[3] < tp_distances[3] + tp_dist_tolerance))
// if ((filtered_val[3] > 700) && (filtered_val[3] < 900))
{
filter_counter[3]++;
if (filter_counter[3] > rdc_val)
{
button_clicked = 3;
print("3 at " + filtered_val[3]);
}
}
else
{
filter_counter[3] = 0;
}
angles_list_3.Clear();
}
}
if ((data[i].theta > tp_angles[4] - tp_angle_tolerance) && (data[i].theta < tp_angles[4] + tp_angle_tolerance))
{
angles_list_4.Add((int)data[i].distant);
// print("data 2: " + data[i].distant);
if (angles_list_4.Count > dist_precision)
{
float avg_val = average(angles_list_4);
filtered_val[4] = (0.8 * filtered_val[4]) + (0.2 * avg_val);
// print(filtered_val[4]);
if ((filtered_val[4] > tp_distances[4] - tp_dist_tolerance) && (filtered_val[4] < tp_distances[4] + tp_dist_tolerance))
{
filter_counter[4]++;
if (filter_counter[4] > rdc_val)
{
button_clicked = 4;
print("4 at " + filtered_val[4]);
}
}
else
{
filter_counter[4] = 0;
}
angles_list_4.Clear();
}
}
if ((data[i].theta > tp_angles[5] - tp_angle_tolerance) && (data[i].theta < tp_angles[5] + tp_angle_tolerance))
{
angles_list_5.Add((int)data[i].distant);
// print("data 2: " + data[i].distant);
if (angles_list_5.Count > dist_precision)
{
float avg_val = average(angles_list_5);
filtered_val[5] = (0.8 * filtered_val[5]) + (0.2 * avg_val);
// print(filtered_val[5]);
if ((filtered_val[5] > 1600) && (filtered_val[5] < 1750))
{
filter_counter[5]++;
// if (filter_counter[5] > rdc_val)
// {
button_clicked = 5;
print("5 at " + filtered_val[5]);
// }
}
else
{
filter_counter[5] = 0;
}
angles_list_5.Clear();
}
}
if ((data[i].theta > tp_angles[6] - tp_angle_tolerance) && (data[i].theta < tp_angles[6] + tp_angle_tolerance))
{
angles_list_6.Add((int)data[i].distant);
// print("data 6: " + data[i].distant);
if (angles_list_6.Count > dist_precision)
{
float avg_val = average(angles_list_6);
filtered_val[6] = (0.8 * filtered_val[6]) + (0.2 * avg_val);
// print(filtered_val[6]);
if ((filtered_val[6] > tp_distances[6] - tp_dist_tolerance) && (filtered_val[6] < tp_distances[6] + tp_dist_tolerance))
{
filter_counter[6]++;
if (filter_counter[6] > rdc_val)
{
button_clicked = 6;
print("6 at " + filtered_val[6]);
}
}
else
{
filter_counter[6] = 0;
}
angles_list_6.Clear();
}
}
if ((data[i].theta > tp_angles[7] - tp_angle_tolerance) && (data[i].theta < tp_angles[7] + tp_angle_tolerance))
{
angles_list_7.Add((int)data[i].distant);
// print("data 7: " + data[i].distant);
if (angles_list_7.Count > dist_precision)
{
float avg_val = average(angles_list_7);
filtered_val[7] = (0.8 * filtered_val[7]) + (0.2 * avg_val);
// print(filtered_val[7]);
if ((filtered_val[7] > tp_distances[7] - tp_dist_tolerance) && (filtered_val[7] < tp_distances[7] + tp_dist_tolerance))
{
filter_counter[7]++;
// if (filter_counter[7] > rdc_val)
// {
button_clicked = 7;
print("7 at " + filtered_val[7]);
// }
}
else
{
filter_counter[7] = 0;
}
angles_list_7.Clear();
}
}
if ((data[i].theta > tp_angles[8] - tp_angle_tolerance) && (data[i].theta < tp_angles[8] + tp_angle_tolerance))
{
angles_list_8.Add((int)data[i].distant);
// print("data 2: " + data[i].distant);
if (angles_list_8.Count > dist_precision)
{
float avg_val = average(angles_list_8);
filtered_val[8] = (0.8 * filtered_val[8]) + (0.2 * avg_val);
// print(filtered_val[8]);
if ((filtered_val[8] > tp_distances[8] - tp_dist_tolerance) && (filtered_val[8] < tp_distances[8] + tp_dist_tolerance))
{
filter_counter[8]++;
// if (filter_counter[8] > rdc_val)
// {
button_clicked = 8;
print("8 at " + filtered_val[8]);
// }
}
else
{
filter_counter[8] = 0;
}
angles_list_8.Clear();
}
}
if ((data[i].theta > tp_angles[9] - tp_angle_tolerance) && (data[i].theta < tp_angles[9] + tp_angle_tolerance))
{
angles_list_9.Add((int)data[i].distant);
// print("data 9: " + data[i].distant);
if (angles_list_9.Count > dist_precision)
{
float avg_val = average(angles_list_9);
filtered_val[9] = (0.9 * filtered_val[9]) + (0.2 * avg_val);
// print(filtered_val[9]);
if ((filtered_val[9] > tp_distances[9] - tp_dist_tolerance) && (filtered_val[9] < tp_distances[9] + tp_dist_tolerance))
// if ((filtered_val[9] > 4000) && (filtered_val[9] < 5000))
{
// filter_counter[9]++;
// if (filter_counter[9] > rdc_val)
// {
button_clicked = 9;
print("9 at " + filtered_val[9]);
// }
}
else
{
filter_counter[9] = 0;
}
angles_list_9.Clear();
}
}
}
}
}
private void OnGUI()
{
DrawButton("Connect", () =>
{
if (string.IsNullOrEmpty(port))
{
return;
}
int result = RplidarBinding.OnConnect(port);
Debug.Log("Connect on " + port + " result:" + result);
});
DrawButton("DisConnect", () =>
{
bool r = RplidarBinding.OnDisconnect();
Debug.Log("Disconnect:" + r);
});
DrawButton("StartScan", () =>
{
bool r = RplidarBinding.StartScan();
Debug.Log("StartScan:" + r);
});
DrawButton("EndScan", () =>
{
bool r = RplidarBinding.EndScan();
Debug.Log("EndScan:" + r);
});
DrawButton("StartMotor", () =>
{
bool r = RplidarBinding.StartMotor();
Debug.Log("StartMotor:" + r);
});
DrawButton("EndMotor", () =>
{
bool r = RplidarBinding.EndMotor();
Debug.Log("EndMotor:" + r);
});
DrawButton("Release Driver", () =>
{
bool r = RplidarBinding.ReleaseDrive();
Debug.Log("Release Driver:" + r);
});
DrawButton("GrabData", () =>
{
int count = RplidarBinding.GetData(ref data);
Debug.Log("GrabData:" + count);
if (count > 0)
{
for (int i = 0; i < 20; i++)
{
Debug.Log("d:" + data[i].distant + " " + data[i].quality + " " + data[i].syncBit + " " + data[i].theta);
}
}
});
}
