Revision beea48aa
Tested forklift code and fixed some things (a few weeks ago)
forklift/code/Makefile | ||
---|---|---|
1 |
SRC = forklift.c analog.c twi.c motor.c |
|
2 |
HDR = analog.h twi.h motor.h |
|
3 |
|
|
1 | 4 |
forklift: forklift.hex |
2 | 5 |
@ |
3 | 6 |
|
4 | 7 |
%.hex: %.elf |
5 | 8 |
avr-objcopy -j .text -j .data -O ihex $< $@ |
6 | 9 |
|
7 |
forklift.elf: forklift.c analog.c analog.h twi.c twi.h
|
|
8 |
avr-gcc -mmcu=atmega168 -DF_CPU=8000000L forklift.c analog.c twi.c -o forklift.elf -Os
|
|
10 |
forklift.elf: $(SRC) $(HDR)
|
|
11 |
avr-gcc -mmcu=atmega168 -DF_CPU=8000000L $(SRC) -o forklift.elf -Os
|
|
9 | 12 |
|
10 | 13 |
download: forklift.hex |
11 |
avrdude -c avrispmkII -p m328p forklift.hex
|
|
14 |
avrdude -c avrispmkII -p m128 -P usb -B 1 -U flash:w:forklift.hex
|
|
12 | 15 |
|
13 | 16 |
clean: |
14 | 17 |
rm -f forklift.elf forklift.hex |
forklift/code/analog.c | ||
---|---|---|
3 | 3 |
|
4 | 4 |
int line_threshold = 150; |
5 | 5 |
|
6 |
int line_read(int which); |
|
7 |
void line_update(int* values); |
|
8 |
int line_locate(int* values); |
|
9 |
|
|
10 | 6 |
void analog_init(void) |
11 | 7 |
{ |
12 | 8 |
// ADMUX register |
... | ... | |
58 | 54 |
return analog_read(ADC_LINE); |
59 | 55 |
} |
60 | 56 |
|
61 |
void line_update(int* values)
|
|
57 |
void line_update(char* values)
|
|
62 | 58 |
{ |
63 | 59 |
int i; |
64 | 60 |
for(i = 0; i<5; i++) |
65 |
values[i] = line_read(i) < line_threshold ? LWHITE : LBLACK;
|
|
61 |
values[i] = line_read(i) < line_threshold ? LBLACK : LWHITE;
|
|
66 | 62 |
} |
67 | 63 |
|
68 |
int line_locate(int* values)
|
|
64 |
int line_locate(char* values)
|
|
69 | 65 |
{ |
70 | 66 |
int i; |
71 | 67 |
int wsum = 0; |
... | ... | |
85 | 81 |
|
86 | 82 |
int line_read_pos(void) |
87 | 83 |
{ |
88 |
int values[5];
|
|
84 |
char values[5];
|
|
89 | 85 |
line_update(values); |
90 | 86 |
return line_locate(values); |
91 | 87 |
} |
forklift/code/analog.h | ||
---|---|---|
16 | 16 |
#define ADC_LINE 1 |
17 | 17 |
|
18 | 18 |
void analog_init(void); |
19 |
int analog_read(int which); |
|
19 |
int analog_read(int which); |
|
20 |
|
|
21 |
void line_update(char* values); |
|
22 |
int line_locate(char* values); |
|
23 |
int line_read(int which); |
|
20 | 24 |
int line_read_pos(void); |
21 | 25 |
void line_set_threshold_high(uint8_t threshhold); |
22 | 26 |
void line_set_threshold_low(uint8_t threshhold); |
forklift/code/forklift.c | ||
---|---|---|
27 | 27 |
#define FORKLIFT_HEIGHT_SETPOINT 3 // r/w |
28 | 28 |
#define FORKLIFT_LINE_POS 4 |
29 | 29 |
#define FORKLIFT_LINE_THRESH_HIGH 5 // r/w |
30 |
#define FORKLIFT_LINE_THRESH_LOW 6 // 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 |
|
31 | 33 |
|
32 |
#define FORKLIFT_DATA_LEN 7
|
|
34 |
#define FORKLIFT_DATA_LEN 12
|
|
33 | 35 |
|
34 | 36 |
|
35 | 37 |
#define PROP(X,Y,Z) (((X)*(Y))/(Z)) |
... | ... | |
42 | 44 |
0, |
43 | 45 |
0, |
44 | 46 |
0, |
45 |
150 |
|
47 |
150, // default line threshold |
|
48 |
0, |
|
49 |
0, |
|
50 |
0, |
|
51 |
0, |
|
52 |
0 |
|
46 | 53 |
}; |
47 | 54 |
int error; |
48 | 55 |
int i_term; |
... | ... | |
84 | 91 |
internal_index++; |
85 | 92 |
if (internal_index >= FORKLIFT_DATA_LEN) |
86 | 93 |
internal_index = 0; |
87 |
} |
|
88 |
|
|
89 |
void update_height(void) |
|
90 |
//Call to update the forklift height point. Should be called each loop |
|
91 |
{ |
|
92 |
internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT); |
|
93 | 94 |
} |
94 | 95 |
|
95 |
|
|
96 | 96 |
int clamp(int min, int max, int val) |
97 | 97 |
{ |
98 | 98 |
if(val>max) return max; |
... | ... | |
105 | 105 |
error = (int)internal_data[FORKLIFT_HEIGHT] |
106 | 106 |
-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT]; |
107 | 107 |
i_term = i_term + error/10; |
108 |
set_motor(clamp(-127,127, i_term/10 + (error*2)/5)); |
|
108 |
//set_motor(clamp(-127,127, i_term/10 + (error*2)/5));
|
|
109 | 109 |
} |
110 | 110 |
|
111 | 111 |
int main() |
112 | 112 |
{ |
113 |
int i; |
|
113 | 114 |
error = 0; |
114 | 115 |
i_term = 0; |
115 | 116 |
init_int0(); |
... | ... | |
122 | 123 |
motor_init(); |
123 | 124 |
while (1) |
124 | 125 |
{ |
125 |
/*error = PROP((int)internal_data[FORKLIFT_HEIGHT] |
|
126 |
-(int)internal_data[FORKLIFT_HEIGHT_SETPOINT],2,5); |
|
127 |
set_motor(clamp(-127,127,error));*/ |
|
128 |
// do motor stuff |
|
129 |
internal_data[FORKLIFT_LINE_POS] = (analog_read(ADC_LINE)>>2);//line_read_pos(); |
|
130 |
internal_data[FORKLIFT_HEIGHT] = (analog_read(ADC_HEIGHT)>>2); |
|
126 |
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(); |
|
129 |
internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT) >> 2; |
|
131 | 130 |
} |
132 | 131 |
return 0; |
133 | 132 |
} |
forklift/code/twi.c | ||
---|---|---|
464 | 464 |
twi_stop(); |
465 | 465 |
break; |
466 | 466 |
} |
467 |
} |
|
468 |
|
|
467 |
} |
Also available in: Unified diff