root / forklift / code / forklift.c @ 902fa82f
History | View | Annotate | Download (2.98 KB)
1 | a52b0259 | Tom Mullins | /*
|
---|---|---|---|
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 | 86196a57 | Tom Mullins | #include "analog.h" |
16 | #include "motor.h" |
||
17 | #include <avr/io.h> |
||
18 | 0bd4bb32 | Tom Mullins | #include <avr/interrupt.h> |
19 | a52b0259 | Tom Mullins | |
20 | #define TRACKING_ID 0x41 |
||
21 | #define SERIAL_NUMBER 0x12 |
||
22 | |||
23 | // indicies for forklift internal data
|
||
24 | 0bd4bb32 | Tom Mullins | #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 | 86196a57 | Tom Mullins | #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 | beea48aa | Tom Mullins | #define FORKLIFT_LINE_VALS_END 12 // non-inclusive |
33 | a52b0259 | Tom Mullins | |
34 | 86196a57 | Tom Mullins | #define FORKLIFT_DATA_LEN 12 |
35 | a52b0259 | Tom Mullins | |
36 | uint8_t internal_index = 0;
|
||
37 | uint8_t internal_data[] = { |
||
38 | TRACKING_ID, |
||
39 | SERIAL_NUMBER, |
||
40 | 0,
|
||
41 | 86196a57 | Tom Mullins | 100, // default height setpoint |
42 | 0,
|
||
43 | 0,
|
||
44 | 150, // default line threshold |
||
45 | 0,
|
||
46 | 0,
|
||
47 | 0,
|
||
48 | a52b0259 | Tom Mullins | 0,
|
49 | beea48aa | Tom Mullins | 0
|
50 | a52b0259 | Tom Mullins | }; |
51 | 86196a57 | Tom Mullins | |
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 | 0bd4bb32 | Tom Mullins | } |
62 | a52b0259 | Tom Mullins | 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 | 86196a57 | Tom Mullins | if (len > 1) |
68 | { |
||
69 | 902fa82f | Tom Mullins | 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 | 86196a57 | Tom Mullins | else if (internal_index == FORKLIFT_LINE_THRESH_LOW) |
80 | 902fa82f | Tom Mullins | { |
81 | internal_data[internal_index] = data[1];
|
||
82 | line_set_threshold_low(data[1]);
|
||
83 | } |
||
84 | 86196a57 | Tom Mullins | } |
85 | 21bf5ee1 | Tom Mullins | } |
86 | a52b0259 | Tom Mullins | } |
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 | 86196a57 | Tom Mullins | } |
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 | 21bf5ee1 | Tom Mullins | { |
105 | // TODO make this more easily tunable (despite integer limitations)
|
||
106 | 86196a57 | Tom Mullins | 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 | a52b0259 | Tom Mullins | |
115 | int main()
|
||
116 | 86196a57 | Tom Mullins | { |
117 | int i;
|
||
118 | error = 0;
|
||
119 | i_term = 0;
|
||
120 | init_int0(); |
||
121 | 0bd4bb32 | Tom Mullins | sei(); |
122 | a52b0259 | Tom Mullins | twi_attachSlaveRxEvent(slave_rx); |
123 | twi_attachSlaveTxEvent(slave_tx); |
||
124 | 86196a57 | Tom Mullins | twi_setAddress(TRACKING_ID); |
125 | 0bd4bb32 | Tom Mullins | twi_init(); |
126 | 86196a57 | Tom Mullins | analog_init(); |
127 | 0bd4bb32 | Tom Mullins | motor_init(); |
128 | a52b0259 | Tom Mullins | while (1) |
129 | 86196a57 | Tom Mullins | { |
130 | for (i = 0; i < 5; i++) |
||
131 | internal_data[i + FORKLIFT_LINE_VALS_START] = line_read(i) >> 2;
|
||
132 | 21bf5ee1 | Tom Mullins | internal_data[FORKLIFT_LINE_POS] = line_read_pos(); |
133 | 86196a57 | Tom Mullins | internal_data[FORKLIFT_HEIGHT] = 255 - (analog_read(ADC_HEIGHT) >> 2); |
134 | a52b0259 | Tom Mullins | } |
135 | return 0; |
||
136 | } |