Revision 86f45a0c
Adding arduino sketch for forklift testing
forklift/code/arduino-test/arduino-test.ino | ||
---|---|---|
1 |
#include <stdlib.h> |
|
2 |
#include <Wire.h> |
|
3 |
|
|
4 |
// A4: SDA |
|
5 |
// A5: SCL |
|
6 |
|
|
7 |
void setup() { |
|
8 |
Serial.begin(115200); |
|
9 |
Wire.begin(); |
|
10 |
} |
|
11 |
|
|
12 |
int readAddr(byte addr) { |
|
13 |
Wire.beginTransmission(0x41); |
|
14 |
Wire.write(addr); |
|
15 |
Wire.endTransmission(); |
|
16 |
Wire.requestFrom(0x41, 1); |
|
17 |
for (int i = 0; !Wire.available(); i++) { |
|
18 |
if (i == 5) return -1; |
|
19 |
delay(1); |
|
20 |
} |
|
21 |
byte ret = Wire.read(); |
|
22 |
return ret; |
|
23 |
} |
|
24 |
|
|
25 |
byte writeAddr(byte addr, byte data) { |
|
26 |
Wire.beginTransmission(0x41); |
|
27 |
Wire.write(addr); |
|
28 |
Wire.write(data); |
|
29 |
Wire.endTransmission(); |
|
30 |
} |
|
31 |
|
|
32 |
int readMany(byte addr) { |
|
33 |
int x; |
|
34 |
while ((x = readAddr(addr)) == -1); |
|
35 |
return x; |
|
36 |
} |
|
37 |
|
|
38 |
char read_buf[5]; |
|
39 |
int read_buf_i = 0; |
|
40 |
|
|
41 |
int checkSerial() { |
|
42 |
while (Serial.available()) { |
|
43 |
char in = Serial.read(); |
|
44 |
if (in == '\n') { |
|
45 |
if (read_buf_i == 4) { |
|
46 |
read_buf_i = 0; |
|
47 |
return -1; // ignore inputs too long |
|
48 |
} |
|
49 |
read_buf[read_buf_i] = 0; |
|
50 |
read_buf_i = 0; |
|
51 |
int n = atoi(read_buf); |
|
52 |
if (n < 0 || n > 255) return -1; |
|
53 |
return n; |
|
54 |
} else if (read_buf_i < 4) { |
|
55 |
read_buf[read_buf_i] = in; |
|
56 |
read_buf_i++; |
|
57 |
} |
|
58 |
} |
|
59 |
return -1; |
|
60 |
} |
|
61 |
|
|
62 |
void loop() { |
|
63 |
int in = checkSerial(); |
|
64 |
/*Serial.println(in); |
|
65 |
delay(650);*/ |
|
66 |
if (in >= 0) |
|
67 |
writeAddr(3, in); |
|
68 |
Serial.print("\nHeight setpoint: "); |
|
69 |
Serial.print(readMany(3)); |
|
70 |
Serial.print("\nHeight: "); |
|
71 |
Serial.print(readMany(2)); |
|
72 |
Serial.print("\n--------------------"); |
|
73 |
delay(650); |
|
74 |
} |
|
75 |
|
Also available in: Unified diff