Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1455 | acid | 1 | |
2 | |||
3 | #include <string.h> |
||
4 | #include <avr/interrupt.h> |
||
5 | #include <avr/eeprom.h> |
||
6 | #include <util/delay.h> |
||
7 | #include "servoboard.h" |
||
8 | #include "twislave.h" |
||
1459 | acid | 9 | #include "uart.h" |
1455 | acid | 10 | |
11 | |||
12 | uint8_t eeprom_neutral_pos[6] EEMEM = { 127, 127, 127, 127, 127, 127 }; |
||
13 | uint16_t eeprom_pwm_limit[6] EEMEM = { 0, 0, 0, 0, 0, 0 }; |
||
1459 | acid | 14 | |
15 | uint8_t pwm_signal[6]; |
||
1455 | acid | 16 | uint8_t pwm_counter = 0; |
1459 | acid | 17 | uint8_t pwm_neutral_position[6]; |
18 | uint8_t pwm_position[6]; |
||
1455 | acid | 19 | uint16_t pwm_limit[6]; |
20 | |||
21 | |||
22 | |||
23 | void pwm_init() { |
||
24 | |||
25 | SERVODDR = 0xff; |
||
26 | |||
27 | set_pwm(); |
||
28 | |||
29 | TCCR1B = (1 << CS11); |
||
30 | TCCR0 = (1 << CS00); |
||
31 | |||
32 | } |
||
33 | |||
34 | void set_pwm() { |
||
35 | |||
36 | for(uint8_t n = 0; n < 6; n++) { |
||
37 | pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n]; |
||
38 | if (pwm_limit[n] != 0) { |
||
39 | uint8_t l, h; |
||
40 | l = pwm_limit[n] & 0xff; |
||
41 | h = pwm_limit[n] >> 8; |
||
42 | if (pwm_signal[n] < l) { |
||
43 | pwm_signal[n] = l; |
||
44 | } |
||
45 | if (pwm_signal[n] > h) { |
||
46 | pwm_signal[n] = h; |
||
47 | } |
||
48 | } |
||
49 | } |
||
50 | |||
51 | } |
||
52 | |||
53 | void set_pwm_neutral() { |
||
54 | |||
55 | for(uint8_t i = 0; i < 6; i++) { |
||
56 | pwm_position[i] = 127; |
||
57 | } |
||
58 | set_pwm(); |
||
59 | |||
60 | } |
||
61 | |||
62 | void eeprom_init() { |
||
63 | |||
64 | eeprom_busy_wait(); |
||
1459 | acid | 65 | eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position)); |
66 | eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit)); |
||
1455 | acid | 67 | |
68 | } |
||
69 | |||
70 | void eeprom_write() { |
||
71 | |||
72 | cli(); |
||
1459 | acid | 73 | eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position)); |
74 | eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit)); |
||
1455 | acid | 75 | sei(); |
76 | |||
77 | } |
||
78 | |||
79 | void delay(int ms) { |
||
80 | |||
81 | while(ms--) { |
||
82 | _delay_ms(1); |
||
83 | } |
||
84 | |||
85 | } |
||
86 | |||
87 | |||
88 | int main(void) { |
||
89 | |||
1456 | acid | 90 | DDRD = 0x80; |
91 | PORTD = 0x80; |
||
1455 | acid | 92 | |
93 | DDRC &= ~0x0f; |
||
94 | PORTC |= 0x0f; |
||
95 | |||
96 | cli(); |
||
1459 | acid | 97 | uart_init(); |
1455 | acid | 98 | eeprom_init(); |
1459 | acid | 99 | i2c_init(0x82); |
1455 | acid | 100 | pwm_init(); |
1456 | acid | 101 | set_pwm_neutral(); |
1455 | acid | 102 | sei(); |
103 | |||
104 | TCNT1 = 0; |
||
105 | while(1) { |
||
106 | |||
107 | cli(); |
||
108 | SERVOPORT = 0xff; |
||
109 | while(TCNT1 < 300) ; |
||
110 | |||
111 | for(uint8_t i = 0; i < 255; i++) { |
||
112 | |||
113 | TCNT0 = 0; |
||
114 | |||
115 | if (i == pwm_signal[0]) { |
||
116 | SERVOPORT &= ~(1<<SERVO1); |
||
117 | } |
||
118 | if (i == pwm_signal[1]) { |
||
119 | SERVOPORT &= ~(1<<SERVO2); |
||
120 | } |
||
121 | if (i == pwm_signal[2]) { |
||
122 | SERVOPORT &= ~(1<<SERVO3); |
||
123 | } |
||
124 | if (i == pwm_signal[3]) { |
||
125 | SERVOPORT &= ~(1<<SERVO4); |
||
126 | } |
||
127 | if (i == pwm_signal[4]) { |
||
128 | SERVOPORT &= ~(1<<SERVO5); |
||
129 | } |
||
130 | if (i == pwm_signal[5]) { |
||
131 | SERVOPORT &= ~(1<<SERVO6); |
||
132 | } |
||
133 | |||
134 | while (TCNT0 < 60) ; |
||
135 | |||
136 | } |
||
137 | sei(); |
||
138 | |||
139 | SERVOPORT = 0; |
||
140 | |||
141 | set_pwm(); |
||
142 | |||
143 | while(TCNT1 < 20000) ; |
||
144 | TCNT1 = 0; |
||
145 | |||
146 | } |
||
147 | |||
148 | return 0; |
||
149 | } |
||
150 |