/
main.c
83 lines (70 loc) · 1.48 KB
/
main.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#include "freepie_io.h"
#include <stdio.h>
#include <string.h>
#include <Windows.h>
#define ARRAY_SIZE 4
void read(int slots, freepie_io_6dof_data *data)
{
int i;
int read_result = freepie_io_6dof_read(0, slots, data);
if(read_result < 0)
{
printf("\nError occured while reading");
return;
}
i = 0;
while(read_result > 0)
{
if(read_result & 1)
printf("\nResult in requested index: %i", i);
read_result >>= 1;
i++;
}
}
void write(int slots, freepie_io_6dof_data *data)
{
if(freepie_io_6dof_write(0, slots, data))
printf("\nError occured while writing");
}
void increment(int count, freepie_io_6dof_data *data)
{
int i;
for(i = 0; i < count; i++)
{
data[i].x += 1e-3f;
data[i].y += 1e-3f;
data[i].z += 1e-3f;
data[i].yaw += 1e-3f;
data[i].pitch += 1e-3f;
data[i].roll += 1e-3f;
}
}
int main()
{
int do_read = 1;
int i = 0;
int count = 0;
uint32_t slots;
freepie_io_6dof_data data[ARRAY_SIZE] = { 0 };
slots = freepie_io_6dof_slots();
if(slots > ARRAY_SIZE)
{
printf("Allocated data too small to accommodate %i slots.", slots);
return 0;
}
if(do_read)
printf("\nAttempting to read...");
else printf("\nAttempting to write...");
while(1)
{
if(do_read)
read(slots, data);
else
{
increment(ARRAY_SIZE, data);
write(slots, data);
}
Sleep(1);
}
printf("\nAll done.");
}