Revision 86196a57 forklift/code/forklift.c

View differences:

forklift/code/forklift.c
12 12
 */
13 13
 
14 14
#include "twi.h"
15
#include "analog.h"

16
#include "motor.h"

17
#include <avr/io.h>

15
#include "analog.h"
16
#include "motor.h"
17
#include <avr/io.h>
18 18
#include <avr/interrupt.h>
19 19

  
20 20
#define TRACKING_ID 0x41
......
25 25
#define FORKLIFT_SERIAL_NUMBER     1
26 26
#define FORKLIFT_HEIGHT            2
27 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

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 32
#define FORKLIFT_LINE_VALS_END    12 // non-inclusive
33 33

  
34
#define FORKLIFT_DATA_LEN         12
35

  
36

  
37
#define PROP(X,Y,Z)                (((X)*(Y))/(Z))
34
#define FORKLIFT_DATA_LEN         12
38 35

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

56

  
57

  
58
void init_int0(void)

59
{

60
  TCCR0B = (1<<CS02)|(1<<CS00); //Timer clock = system clock / 1024

61
  TIFR0 = 1<<TOV0; //Clear TOV0  clear pending interrupts

62
  TIMSK0 = 1<<TOIE0; //Enable Timer0 Overflow Interrupt

63
  
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
  
64 61
}
65 62
void slave_rx(uint8_t* data, int len)
66 63
{
67 64
  if (len > 0 && data[0] < FORKLIFT_DATA_LEN)
68 65
  {
69 66
    internal_index = data[0];
70
    if (len > 1)
71
  {
72
    if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
73
        internal_data[internal_index] = data[1];
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
    }
67
    if (len > 1)
68
  {
69
    if (internal_index == FORKLIFT_HEIGHT_SETPOINT) {
70
        internal_data[internal_index] = data[1];
71
        i_term = 0;
72
    }
73
    else if (internal_index == FORKLIFT_LINE_THRESH_HIGH)
74
    {
75
      internal_data[internal_index] = data[1];
76
    line_set_threshold_high(data[1]);
77
    }
78
      else if (internal_index == FORKLIFT_LINE_THRESH_LOW)
79
    {
80
      internal_data[internal_index] = data[1];
81
    line_set_threshold_low(data[1]);
82
    }
84 83
  }
85 84
  }
86 85
}
......
91 90
  internal_index++;
92 91
  if (internal_index >= FORKLIFT_DATA_LEN)
93 92
    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)

93
}
94

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

  
102
SIGNAL(TIMER0_OVF_vect)
104 103
{
105 104
  // 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/10;
109
  int speed = clamp(-127, 127, i_term/10 + (error*2)/5);
110
  if (height < 10 && speed < 0) speed = 0;
111
  if (height > 245 && speed > 0) speed = 0;

112
  set_motor(speed);

113
}

105
  int height = (int)internal_data[FORKLIFT_HEIGHT];
106
  error = (int)internal_data[FORKLIFT_HEIGHT_SETPOINT] - height;
107
  i_term = i_term + error/4;
108
  int speed = clamp(-127, 127, i_term/12 + error*4);
109
  if (height < 15 && speed < 0) speed = 0;
110
  if (height > 240 && speed > 0) speed = 0;
111
  set_motor(speed);
112
}
114 113

  
115 114
int main()
116
{

117
  int i;

118
  error = 0;

119
  i_term = 0;

120
  init_int0();

115
{
116
  int i;
117
  error = 0;
118
  i_term = 0;
119
  init_int0();
121 120
  sei();
122 121
  twi_attachSlaveRxEvent(slave_rx);
123 122
  twi_attachSlaveTxEvent(slave_tx);
124
  twi_setAddress(TRACKING_ID);

123
  twi_setAddress(TRACKING_ID);
125 124
  twi_init();
126
  analog_init();

125
  analog_init();
127 126
  motor_init();
128 127
  while (1)
129
  {

130
    for (i = 0; i < 5; i++)

131
      internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;

128
  {
129
    for (i = 0; i < 5; i++)
130
      internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
132 131
    internal_data[FORKLIFT_LINE_POS] = line_read_pos();
133
    internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT) >> 2;
132
    internal_data[FORKLIFT_HEIGHT] = 255 - (analog_read(ADC_HEIGHT) >> 2);
134 133
  }
135 134
  return 0;
136 135
}

Also available in: Unified diff