Revision 21bf5ee1
Added safety limits for the forklift potentiometer
forklift/code/forklift.c | ||
---|---|---|
68 | 68 |
{ |
69 | 69 |
internal_index = data[0]; |
70 | 70 |
if (len > 1) |
71 |
{
|
|
72 |
if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
|
|
71 |
{
|
|
72 |
if (internal_index == FORKLIFT_HEIGHT_SETPOINT)
|
|
73 | 73 |
internal_data[internal_index] = data[1]; |
74 | 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 |
}
|
|
75 |
{
|
|
76 |
internal_data[internal_index] = data[1];
|
|
77 |
line_set_threshold_high(data[1]);
|
|
78 |
}
|
|
79 | 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 |
}
|
|
80 |
{
|
|
81 |
internal_data[internal_index] = data[1];
|
|
82 |
line_set_threshold_low(data[1]);
|
|
83 |
}
|
|
84 |
}
|
|
85 | 85 |
} |
86 | 86 |
} |
87 | 87 |
|
... | ... | |
95 | 95 |
|
96 | 96 |
int clamp(int min, int max, int val) |
97 | 97 |
{ |
98 |
if(val>max) return max;
|
|
99 |
if(val<min) return min;
|
|
100 |
return val;
|
|
98 |
if (val > max) return max;
|
|
99 |
if (val < min) return min;
|
|
100 |
return val;
|
|
101 | 101 |
} |
102 | 102 |
|
103 | 103 |
SIGNAL(TIMER0_OVF_vect) |
104 |
{ |
|
105 |
error = (int)internal_data[FORKLIFT_HEIGHT] |
|
106 |
-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT]; |
|
107 |
i_term = i_term + error/10; |
|
108 |
//set_motor(clamp(-127,127, i_term/10 + (error*2)/5)); |
|
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/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); |
|
109 | 113 |
} |
110 | 114 |
|
111 | 115 |
int main() |
... | ... | |
124 | 128 |
while (1) |
125 | 129 |
{ |
126 | 130 |
for (i = 0; i < 5; i++) |
127 |
internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
|
|
128 |
internal_data[FORKLIFT_LINE_POS] = line_read_pos();
|
|
131 |
internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
|
|
132 |
internal_data[FORKLIFT_LINE_POS] = line_read_pos();
|
|
129 | 133 |
internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT) >> 2; |
130 | 134 |
} |
131 | 135 |
return 0; |
Also available in: Unified diff