Statistics
| Revision:

root / trunk / code / projects / forklift_demo / Forklift.cpp @ 2011

History | View | Annotate | Download (2.05 KB)

1
#define USE_ALT_TWI // an alternative to libdragonfly's i2c code, in twi.cpp
2

    
3
#include "Forklift.h"
4
extern "C"
5
{
6
#include <util/delay.h>
7
}
8

    
9
#ifdef USE_ALT_TWI
10
#  include "twi.h"
11
#else
12
extern "C"
13
{
14
#  include "i2c.h"
15
}
16
#endif
17

    
18
#define SCOUT_AVR_ADDR 0x01
19

    
20
#define FORKLIFT_ADDR 0x41
21

    
22
// indicies for forklift internal data
23
#define FORKLIFT_TRACKING_ID       0
24
#define FORKLIFT_SERIAL_NUMBER     1
25
#define FORKLIFT_HEIGHT            2
26
#define FORKLIFT_HEIGHT_SETPOINT   3 // r/w
27
#define FORKLIFT_LINE_POS          4
28
#define FORKLIFT_LINE_THRESH_HIGH  5 // r/w
29
#define FORKLIFT_LINE_THRESH_LOW   6 // r/w
30

    
31
#define FORKLIFT_DATA_LEN          7
32

    
33
char volatile response;
34
char volatile received;
35

    
36
static int mrecv(char data)
37
{
38
  response = data;
39
  received = 1;
40
  return 0;
41
}
42

    
43
static void srecv(char) {}
44
static char ssend() {return 0;}
45

    
46
static int read_addr(char addr)
47
{
48
#ifdef USE_ALT_TWI
49
  return FORKLIFT_ERROR;
50
#else
51
  if (i2c_send(FORKLIFT_ADDR, &addr, 1))
52
    return FORKLIFT_ERROR;
53
  received = 0;
54
  if (i2c_request(FORKLIFT_ADDR))
55
    return FORKLIFT_ERROR;
56
  for (int i = 0; i < 500; i++) // waits up to ~50 ms
57
  {
58
    _delay_ms(.1);
59
    if (received)
60
      return response;
61
  }
62
  return FORKLIFT_ERROR;
63
#endif
64
}
65

    
66
static int write_addr(char addr, char data)
67
{
68
  char buf[2];
69
  buf[0] = addr;
70
  buf[1] = data;
71
#ifdef USE_ALT_TWI
72
  if (twi_writeTo(FORKLIFT_ADDR, (uint8_t*) buf, 2, 1))
73
    return FORKLIFT_ERROR;
74
#else
75
  if (i2c_send(FORKLIFT_ADDR, buf, 2))
76
    return FORKLIFT_ERROR;
77
#endif
78
  return 0;
79
}
80

    
81
void Forklift::forklift_init()
82
{
83
#ifdef USE_ALT_TWI
84
  twi_init();
85
  twi_setAddress(SCOUT_AVR_ADDR);
86
#else
87
  i2c_init(SCOUT_AVR_ADDR, mrecv, srecv, ssend);
88
#endif
89
}
90

    
91
int Forklift::get_tracking_id()
92
{
93
  return read_addr(FORKLIFT_TRACKING_ID);
94
}
95

    
96
int Forklift::get_serial_number()
97
{
98
  return read_addr(FORKLIFT_SERIAL_NUMBER);
99
}
100

    
101
int Forklift::get_height()
102
{
103
  return read_addr(FORKLIFT_HEIGHT);
104
}
105

    
106
int Forklift::get_height_setpoint()
107
{
108
  return read_addr(FORKLIFT_HEIGHT_SETPOINT);
109
}
110

    
111
int Forklift::set_height_setpoint(char setpoint)
112
{
113
  return write_addr(FORKLIFT_HEIGHT_SETPOINT, setpoint);
114
}