Statistics
| Branch: | Revision:

scoutece / forklift / code / forklift.c @ 902fa82f

History | View | Annotate | Download (2.98 KB)

1
/*
2
 * ARM -> us
3
 * -height setpoint
4
 *
5
 * us -> ARM
6
 * -tracking ID (same for all forklifts)
7
 * -serial number (unique to each forklift)
8
 * -current height
9
 * -current height setpoint
10
 * -payload weight
11
 *
12
 */
13
 
14
#include "twi.h"
15
#include "analog.h"
16
#include "motor.h"
17
#include <avr/io.h>
18
#include <avr/interrupt.h>
19

    
20
#define TRACKING_ID 0x41
21
#define SERIAL_NUMBER 0x12
22

    
23
// indicies for forklift internal data
24
#define FORKLIFT_TRACKING_ID       0
25
#define FORKLIFT_SERIAL_NUMBER     1
26
#define FORKLIFT_HEIGHT            2
27
#define FORKLIFT_HEIGHT_SETPOINT   3 // r/w
28
#define FORKLIFT_LINE_POS          4
29
#define FORKLIFT_LINE_THRESH_HIGH  5 // r/w
30
#define FORKLIFT_LINE_THRESH_LOW   6 // r/w
31
#define FORKLIFT_LINE_VALS_START   7
32
#define FORKLIFT_LINE_VALS_END    12 // non-inclusive
33

    
34
#define FORKLIFT_DATA_LEN         12
35

    
36
uint8_t internal_index = 0;
37
uint8_t internal_data[] = {
38
  TRACKING_ID,
39
  SERIAL_NUMBER,
40
  0,
41
  100, // default height setpoint
42
  0,
43
  0,
44
  150, // default line threshold
45
  0,
46
  0,
47
  0,
48
  0,
49
  0
50
};
51

    
52
int error;
53
int i_term;
54

    
55
void init_int0(void)
56
{
57
  TCCR0B = (1<<CS02)|(1<<CS00); //Timer clock = system clock / 1024
58
  TIFR0 = 1<<TOV0; //Clear TOV0  clear pending interrupts
59
  TIMSK0 = 1<<TOIE0; //Enable Timer0 Overflow Interrupt
60
  
61
}
62
void slave_rx(uint8_t* data, int len)
63
{
64
  if (len > 0 && data[0] < FORKLIFT_DATA_LEN)
65
  {
66
    internal_index = data[0];
67
    if (len > 1)
68
    {
69
      if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
70
      {
71
          internal_data[internal_index] = data[1];
72
          i_term = 0;
73
      }
74
      else if (internal_index == FORKLIFT_LINE_THRESH_HIGH)
75
      {
76
        internal_data[internal_index] = data[1];
77
        line_set_threshold_high(data[1]);
78
      }
79
      else if (internal_index == FORKLIFT_LINE_THRESH_LOW)
80
      {
81
        internal_data[internal_index] = data[1];
82
        line_set_threshold_low(data[1]);
83
      }
84
    }
85
  }
86
}
87

    
88
void slave_tx(void)
89
{
90
  twi_transmit(&internal_data[internal_index], 1);
91
  internal_index++;
92
  if (internal_index >= FORKLIFT_DATA_LEN)
93
    internal_index = 0;
94
}
95

    
96
int clamp(int min, int max, int val)
97
{
98
  if (val > max) return max;
99
  if (val < min) return min;
100
  return val;
101
}
102

    
103
SIGNAL(TIMER0_OVF_vect)
104
{
105
  // TODO make this more easily tunable (despite integer limitations)
106
  int height = (int)internal_data[FORKLIFT_HEIGHT];
107
  error = (int)internal_data[FORKLIFT_HEIGHT_SETPOINT] - height;
108
  i_term = i_term + error/4;
109
  int speed = clamp(-127, 127, i_term/12 + error*4);
110
  if (height < 15 && speed < 0) speed = 0;
111
  if (height > 240 && speed > 0) speed = 0;
112
  set_motor(speed);
113
}
114

    
115
int main()
116
{
117
  int i;
118
  error = 0;
119
  i_term = 0;
120
  init_int0();
121
  sei();
122
  twi_attachSlaveRxEvent(slave_rx);
123
  twi_attachSlaveTxEvent(slave_tx);
124
  twi_setAddress(TRACKING_ID);
125
  twi_init();
126
  analog_init();
127
  motor_init();
128
  while (1)
129
  {
130
    for (i = 0; i < 5; i++)
131
      internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
132
    internal_data[FORKLIFT_LINE_POS] = line_read_pos();
133
    internal_data[FORKLIFT_HEIGHT] = 255 - (analog_read(ADC_HEIGHT) >> 2);
134
  }
135
  return 0;
136
}