root / forklift / code / arduino-test / arduino-test.ino @ 86f45a0c
History | View | Annotate | Download (1.38 KB)
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 |
|