-
Notifications
You must be signed in to change notification settings - Fork 0
/
test.c
64 lines (51 loc) · 1.59 KB
/
test.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
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
extern void motorImplementationInitialize(int motorPins[], int motorMax) ;
extern void motorImplementationFinalize(int motorPins[], int motorMax) ;
extern void motorImplementationSendThrottles(int motorPins[], int motorMax, double motorThrottle[]) ;
#define MAXN 40
int motorPins[MAXN];
double throttles[MAXN];
int main(int argc, char **argv) {
int i, n;
if (argc < 2) {
printf("usage: ./test <gpio_pin_1> ... <gpio_pin_n>\n");
printf("example: sudo ./test 16 19 20 21\n");
exit(1);
}
n = 0;
for(i=1; i<argc; i++) {
motorPins[n] = atoi(argv[i]);
if (motorPins[n] >= 8 && motorPins[n] <= 25) {
n ++;
} else {
printf("pin %d out of range 8..25\n", motorPins[n]);
}
}
motorImplementationInitialize(motorPins, n);
printf("Initializing ESC / Arm, waiting 5 seconds.\n");
fflush(stdout);
// send 0 throttle during 5 seconds
for(i=0; i<n; i++) throttles[i] = 0;
for(i=0; i<5000; i++) {
motorImplementationSendThrottles(motorPins, n, throttles);
usleep(1000);
}
printf("Spinning.\n");
fflush(stdout);
// make motors spinning on 15% throttle during 5 seconds
for(i=0; i<n; i++) throttles[i] = 0.15;
for(i=0; i<5000; i++) {
motorImplementationSendThrottles(motorPins, n, throttles);
usleep(1000);
}
printf("Stop.\n");
fflush(stdout);
// stop motors
for(i=0; i<n; i++) throttles[i] = 0;
motorImplementationSendThrottles(motorPins, n, throttles);
// finalize
motorImplementationFinalize(motorPins, n);
return(0);
}