Revision 86f45a0c

View differences:

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