root / forklift / code / forklift.c @ beea48aa
History | View | Annotate | Download (2.78 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 | 0bd4bb32 | Tom Mullins | #include "analog.h" |
16 | #include "motor.h" |
||
17 | #include <avr/io.h> |
||
18 | #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 | #define FORKLIFT_LINE_POS 4 |
||
29 | #define FORKLIFT_LINE_THRESH_HIGH 5 // r/w |
||
30 | beea48aa | Tom Mullins | #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 | a52b0259 | Tom Mullins | |
34 | beea48aa | Tom Mullins | #define FORKLIFT_DATA_LEN 12 |
35 | 0bd4bb32 | Tom Mullins | |
36 | |||
37 | #define PROP(X,Y,Z) (((X)*(Y))/(Z))
|
||
38 | a52b0259 | Tom Mullins | |
39 | uint8_t internal_index = 0;
|
||
40 | uint8_t internal_data[] = { |
||
41 | TRACKING_ID, |
||
42 | SERIAL_NUMBER, |
||
43 | 0,
|
||
44 | 0,
|
||
45 | 0bd4bb32 | Tom Mullins | 0,
|
46 | 0,
|
||
47 | beea48aa | Tom Mullins | 150, // default line threshold |
48 | 0,
|
||
49 | 0,
|
||
50 | 0,
|
||
51 | 0,
|
||
52 | 0
|
||
53 | a52b0259 | Tom Mullins | }; |
54 | 0bd4bb32 | Tom Mullins | 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 | |||
64 | } |
||
65 | a52b0259 | Tom Mullins | void slave_rx(uint8_t* data, int len) |
66 | { |
||
67 | if (len > 0 && data[0] < FORKLIFT_DATA_LEN) |
||
68 | { |
||
69 | internal_index = data[0];
|
||
70 | 0bd4bb32 | Tom Mullins | 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 | } |
||
84 | } |
||
85 | a52b0259 | Tom Mullins | } |
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 | 0bd4bb32 | 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 | { |
||
105 | error = (int)internal_data[FORKLIFT_HEIGHT]
|
||
106 | -(int)internal_data[FORKLIFT_HEIGHT_SETPOINT];
|
||
107 | i_term = i_term + error/10;
|
||
108 | beea48aa | Tom Mullins | //set_motor(clamp(-127,127, i_term/10 + (error*2)/5));
|
109 | 0bd4bb32 | Tom Mullins | } |
110 | a52b0259 | Tom Mullins | |
111 | int main()
|
||
112 | 0bd4bb32 | Tom Mullins | { |
113 | beea48aa | Tom Mullins | int i;
|
114 | 0bd4bb32 | Tom Mullins | error = 0;
|
115 | i_term = 0;
|
||
116 | init_int0(); |
||
117 | sei(); |
||
118 | a52b0259 | Tom Mullins | twi_attachSlaveRxEvent(slave_rx); |
119 | twi_attachSlaveTxEvent(slave_tx); |
||
120 | 0bd4bb32 | Tom Mullins | twi_setAddress(TRACKING_ID); |
121 | twi_init(); |
||
122 | analog_init(); |
||
123 | motor_init(); |
||
124 | a52b0259 | Tom Mullins | while (1) |
125 | 0bd4bb32 | Tom Mullins | { |
126 | beea48aa | Tom Mullins | 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;
|
||
130 | a52b0259 | Tom Mullins | } |
131 | return 0; |
||
132 | } |