WebSVN - FlightCtrl - Diff - Rev 1464 and 1465 - /branches/V0.76g-acid/servoboard/servoboard.c


 
#include <string.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <util/delay.h>
#include "servoboard.h"
#include "twislave.h"
#include "uart.h"
 
 
uint8_t eeprom_neutral_pos[6] EEMEM;
uint16_t eeprom_pwm_limit[6] EEMEM;
 
uint8_t pwm_signal[6];
uint8_t pwm_neutral_position[6];
uint8_t pwm_position[6];
uint16_t pwm_limit[6];
uint8_t pwm_active;
uint8_t pwm_status = 1;
 
 
void pwm_init() {
 
        SERVODDR = 0xff;
 
        set_pwm();
 
        TCCR1B = (1 << CS11);
        TCCR0 = (1 << CS00);
 
}
 
void set_pwm() {
 
        for(uint8_t n = 0; n < 6; n++) {
                pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];
                if (pwm_limit[n] != 0) {
                        uint8_t l, h;
                        l = pwm_limit[n] & 0xff;
                        h = pwm_limit[n] >> 8;
                        if (pwm_signal[n] < l) {
                                pwm_signal[n] = l;
                        }
                        if (pwm_signal[n] > h) {
                                pwm_signal[n] = h;
                        }
                }
        }
 
}
 
void set_pwm_neutral() {
 
        for(uint8_t i = 0; i < 6; i++) {
                pwm_position[i] = 127;
        }
        set_pwm();
 
}
 
void pwm_check_active() {
 
        // check if pwm is configured
        pwm_active  = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0;
        pwm_active |= (pwm_limit[1] != 0xffff) ? (1 << SERVO2) : 0;
        pwm_active |= (pwm_limit[2] != 0xffff) ? (1 << SERVO3) : 0;
        pwm_active |= (pwm_limit[3] != 0xffff) ? (1 << SERVO4) : 0;
        pwm_active |= (pwm_limit[4] != 0xffff) ? (1 << SERVO5) : 0;
        pwm_active |= (pwm_limit[5] != 0xffff) ? (1 << SERVO6) : 0;
 
}
 
 
void eeprom_init() {
 
        eeprom_busy_wait();
        eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
        eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
        pwm_check_active();
 
}
 
void eeprom_write() {
 
        cli();
        eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
        eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
        sei();
 
}
 
void delay(int ms) {
 
        while(ms--) {
                _delay_ms(1);
        }
 
}
 
 
int main(void) {
 
        uint8_t blink_counter = 0;
        uint8_t blink = 0;
 
        // intialize
        DDRD = 0x80;
        PORTD = 0x80;
 
        DDRC &= ~0x0f;
        PORTC |= 0x0f;
 
        cli();
        uart_init();
        eeprom_init();
        i2c_init(I2C_ADDRESS);
        pwm_init();
        set_pwm_neutral();
        sei();
 
        // start pwm
        TCNT1 = 0;
        while(1) {
 
                cli();
                SERVOPORT = pwm_active;
 
                // show status
                blink_counter++;
 
                if (pwm_active == 0) { // not configured
 
                        blink = 50;
                        pwm_status = 1;
 
                } else {
 
                        if (I2C_timeout == 0) { // no i2c signal
                                blink = 0;
                                pwm_status = 2;
                        } else {
                                I2C_timeout--;
                                if (I2C_timeout == 0) {
                                        set_pwm_neutral();
                                        set_pwm();
                                }
                                blink = 5;
                                pwm_status = 0;
                        }
 
                }
 
                if (blink == 0) {
                        LED_ON;
                } else if (blink_counter % blink == 0) {
                        blink_counter = 1;
                        if (LED_IS_ON) {
                                LED_OFF;
                        } else {
                                LED_ON;
                        }
                }
 
                // wait with high pwm signal
                while(TCNT1 < 300) ;
 
                // check servos setting pwm to low
                for(uint8_t i = 0; i < 255; i++) {
 
                        TCNT0 = 0;
 
                        if (i == pwm_signal[0]) {
                                SERVOPORT &= ~(1<<SERVO1);
                        }
                        if (i == pwm_signal[1]) {
                                SERVOPORT &= ~(1<<SERVO2);
                        }
                        if (i == pwm_signal[2]) {
                                SERVOPORT &= ~(1<<SERVO3);
                        }
                        if (i == pwm_signal[3]) {
                                SERVOPORT &= ~(1<<SERVO4);
                        }
                        if (i == pwm_signal[4]) {
                                SERVOPORT &= ~(1<<SERVO5);
                        }
                        if (i == pwm_signal[5]) {
                                SERVOPORT &= ~(1<<SERVO6);
                        }
 
                        while (TCNT0 < 60) ;
 
                }
                sei();
 
                // set all servos to low
                SERVOPORT = 0;
 
                // update signals
                set_pwm();
 
                // wait till 20ms are reached
                while(TCNT1 < 20000) ;
                TCNT1 = 0;
 
        }
 
        return 0;
}
 
 

Rev 1464	Rev 1465
1		1
2	#include <string.h>	2	#include <string.h>
3	#include <avr/interrupt.h>	3	#include <avr/interrupt.h>
4	#include <avr/eeprom.h>	4	#include <avr/eeprom.h>
5	#include <util/delay.h>	5	#include <util/delay.h>
6	#include "servoboard.h"	6	#include "servoboard.h"
7	#include "twislave.h"	7	#include "twislave.h"
8	#include "uart.h"	8	#include "uart.h"
9		9
10		10
11	uint8_t eeprom_neutral_pos[6] EEMEM;	11	uint8_t eeprom_neutral_pos[6] EEMEM;
12	uint16_t eeprom_pwm_limit[6] EEMEM;	12	uint16_t eeprom_pwm_limit[6] EEMEM;
13		13
14	volatile uint8_t pwm_signal[6];	14	uint8_t pwm_signal[6];
15	volatile uint8_t pwm_neutral_position[6];	15	uint8_t pwm_neutral_position[6];
16	volatile uint8_t pwm_position[6];	16	uint8_t pwm_position[6];
17	volatile uint16_t pwm_limit[6];	17	uint16_t pwm_limit[6];
-		18	uint8_t pwm_active;
18	volatile uint8_t pwm_active;	19	uint8_t pwm_status = 1;
19		20
20		21
21	void pwm_init() {	22	void pwm_init() {
22		23
23	SERVODDR = 0xff;	24	SERVODDR = 0xff;
24		25
25	set_pwm();	26	set_pwm();
26		27
27	TCCR1B = (1 << CS11);	28	TCCR1B = (1 << CS11);
28	TCCR0 = (1 << CS00);	29	TCCR0 = (1 << CS00);
29		30
30	}	31	}
31		32
32	void set_pwm() {	33	void set_pwm() {
33		34
34	for(uint8_t n = 0; n < 6; n++) {	35	for(uint8_t n = 0; n < 6; n++) {
35	pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];	36	pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];
36	if (pwm_limit[n] != 0) {	37	if (pwm_limit[n] != 0) {
37	uint8_t l, h;	38	uint8_t l, h;
38	l = pwm_limit[n] & 0xff;	39	l = pwm_limit[n] & 0xff;
39	h = pwm_limit[n] >> 8;	40	h = pwm_limit[n] >> 8;
40	if (pwm_signal[n] < l) {	41	if (pwm_signal[n] < l) {
41	pwm_signal[n] = l;	42	pwm_signal[n] = l;
42	}	43	}
43	if (pwm_signal[n] > h) {	44	if (pwm_signal[n] > h) {
44	pwm_signal[n] = h;	45	pwm_signal[n] = h;
45	}	46	}
46	}	47	}
47	}	48	}
48		49
49	}	50	}
50		51
51	void set_pwm_neutral() {	52	void set_pwm_neutral() {
52		53
53	for(uint8_t i = 0; i < 6; i++) {	54	for(uint8_t i = 0; i < 6; i++) {
54	pwm_position[i] = 127;	55	pwm_position[i] = 127;
55	}	56	}
56	set_pwm();	57	set_pwm();
57		58
58	}	59	}
59		60
60	void pwm_check_active() {	61	void pwm_check_active() {
61		62
62	// check if pwm is configured	63	// check if pwm is configured
63	pwm_active = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0;	64	pwm_active = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0;
64	pwm_active \|= (pwm_limit[1] != 0xffff) ? (2 << SERVO1) : 0;	65	pwm_active \|= (pwm_limit[1] != 0xffff) ? (1 << SERVO2) : 0;
65	pwm_active \|= (pwm_limit[2] != 0xffff) ? (3 << SERVO1) : 0;	66	pwm_active \|= (pwm_limit[2] != 0xffff) ? (1 << SERVO3) : 0;
66	pwm_active \|= (pwm_limit[3] != 0xffff) ? (4 << SERVO1) : 0;	67	pwm_active \|= (pwm_limit[3] != 0xffff) ? (1 << SERVO4) : 0;
67	pwm_active \|= (pwm_limit[4] != 0xffff) ? (5 << SERVO1) : 0;	68	pwm_active \|= (pwm_limit[4] != 0xffff) ? (1 << SERVO5) : 0;
68	pwm_active \|= (pwm_limit[5] != 0xffff) ? (6 << SERVO1) : 0;	69	pwm_active \|= (pwm_limit[5] != 0xffff) ? (1 << SERVO6) : 0;
69		70
70	}	71	}
71		72
72		73
73	void eeprom_init() {	74	void eeprom_init() {
74		75
75	eeprom_busy_wait();	76	eeprom_busy_wait();
76	eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));	77	eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
77	eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));	78	eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
78	pwm_check_active();	79	pwm_check_active();
79		80
80	}	81	}
81		82
82	void eeprom_write() {	83	void eeprom_write() {
83		84
84	cli();	85	cli();
85	eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));	86	eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
86	eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));	87	eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
87	sei();	88	sei();
88		89
89	}	90	}
90		91
91	void delay(int ms) {	92	void delay(int ms) {
92		93
93	while(ms--) {	94	while(ms--) {
94	_delay_ms(1);	95	_delay_ms(1);
95	}	96	}
96		97
97	}	98	}
98		99
99		100
100	int main(void) {	101	int main(void) {
101		102
102	uint8_t blink_counter = 0;	103	uint8_t blink_counter = 0;
103	uint8_t blink = 0;	104	uint8_t blink = 0;
104		105
105	// intialize	106	// intialize
106	DDRD = 0x80;	107	DDRD = 0x80;
107	PORTD = 0x80;	108	PORTD = 0x80;
108		109
109	DDRC &= ~0x0f;	110	DDRC &= ~0x0f;
110	PORTC \|= 0x0f;	111	PORTC \|= 0x0f;
111		112
112	cli();	113	cli();
113	uart_init();	114	uart_init();
114	eeprom_init();	115	eeprom_init();
115	i2c_init(I2C_ADDRESS);	116	i2c_init(I2C_ADDRESS);
116	pwm_init();	117	pwm_init();
117	set_pwm_neutral();	118	set_pwm_neutral();
118	sei();	119	sei();
119		120
120	// start pwm	121	// start pwm
121	TCNT1 = 0;	122	TCNT1 = 0;
122	while(1) {	123	while(1) {
123		124
124	cli();	125	cli();
125	SERVOPORT = pwm_active;	126	SERVOPORT = pwm_active;
126		127
127	// show status	128	// show status
128	blink_counter++;	129	blink_counter++;
129		130
130	if (pwm_active == 0) { // not configured	131	if (pwm_active == 0) { // not configured
131		132
132	blink = 50;	133	blink = 50;
-		134	pwm_status = 1;
133		135
134	} else {	136	} else {
135		137
136	if (I2C_timeout == 0) { // no i2c signal	138	if (I2C_timeout == 0) { // no i2c signal
137	blink = 0;	139	blink = 0;
-		140	pwm_status = 2;
138	} else {	141	} else {
139	I2C_timeout--;	142	I2C_timeout--;
-		143	if (I2C_timeout == 0) {
-		144	set_pwm_neutral();
-		145	set_pwm();
-		146	}
140	blink = 5;	147	blink = 5;
-		148	pwm_status = 0;
141	}	149	}
142		150
143	}	151	}
144		152
145	if (blink == 0) {	153	if (blink == 0) {
146	LED_ON;	154	LED_ON;
147	} else if (blink_counter % blink == 0) {	155	} else if (blink_counter % blink == 0) {
148	blink_counter = 1;	156	blink_counter = 1;
149	if (LED_IS_ON) {	157	if (LED_IS_ON) {
150	LED_OFF;	158	LED_OFF;
151	} else {	159	} else {
152	LED_ON;	160	LED_ON;
153	}	161	}
154	}	162	}
155		163
156	// wait with high pwm signal	164	// wait with high pwm signal
157	while(TCNT1 < 300) ;	165	while(TCNT1 < 300) ;
158		166
159	// check servos setting pwm to low	167	// check servos setting pwm to low
160	for(uint8_t i = 0; i < 255; i++) {	168	for(uint8_t i = 0; i < 255; i++) {
161		169
162	TCNT0 = 0;	170	TCNT0 = 0;
163		171
164	if (i == pwm_signal[0]) {	172	if (i == pwm_signal[0]) {
165	SERVOPORT &= ~(1<<SERVO1);	173	SERVOPORT &= ~(1<<SERVO1);
166	}	174	}
167	if (i == pwm_signal[1]) {	175	if (i == pwm_signal[1]) {
168	SERVOPORT &= ~(1<<SERVO2);	176	SERVOPORT &= ~(1<<SERVO2);
169	}	177	}
170	if (i == pwm_signal[2]) {	178	if (i == pwm_signal[2]) {
171	SERVOPORT &= ~(1<<SERVO3);	179	SERVOPORT &= ~(1<<SERVO3);
172	}	180	}
173	if (i == pwm_signal[3]) {	181	if (i == pwm_signal[3]) {
174	SERVOPORT &= ~(1<<SERVO4);	182	SERVOPORT &= ~(1<<SERVO4);
175	}	183	}
176	if (i == pwm_signal[4]) {	184	if (i == pwm_signal[4]) {
177	SERVOPORT &= ~(1<<SERVO5);	185	SERVOPORT &= ~(1<<SERVO5);
178	}	186	}
179	if (i == pwm_signal[5]) {	187	if (i == pwm_signal[5]) {
180	SERVOPORT &= ~(1<<SERVO6);	188	SERVOPORT &= ~(1<<SERVO6);
181	}	189	}
182		190
183	while (TCNT0 < 60) ;	191	while (TCNT0 < 60) ;
184		192
185	}	193	}
186	sei();	194	sei();
187		195
188	// set all servos to low	196	// set all servos to low
189	SERVOPORT = 0;	197	SERVOPORT = 0;
190		198
191	// update signals	199	// update signals
192	set_pwm();	200	set_pwm();
193		201
194	// wait till 20ms are reached	202	// wait till 20ms are reached
195	while(TCNT1 < 20000) ;	203	while(TCNT1 < 20000) ;
196	TCNT1 = 0;	204	TCNT1 = 0;
197		205
198	}	206	}
199		207
200	return 0;	208	return 0;
201	}	209	}
202		210
203		211

Subversion Repositories FlightCtrl

(root)/branches/V0.76g-acid/servoboard/servoboard.c - Rev 1464 → 1465