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