Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1458 → Rev 1459

/branches/V0.76g-acid/servoboard/readme.txt
6,6 → 6,8
GND
VCC
RESET
SCL
SDA
 
LED an PD7
 
/branches/V0.76g-acid/servoboard/servoboard.c
1,5 → 1,4
 
#define F_CPU 8000000UL
 
#include <string.h>
#include <avr/interrupt.h>
7,25 → 6,16
#include <util/delay.h>
#include "servoboard.h"
#include "twislave.h"
#include "uart.h"
 
 
#define SERVODDR DDRB
#define SERVOPORT PORTB
#define SERVO1 PB0
#define SERVO2 PB1
#define SERVO3 PB2
#define SERVO4 PB3
#define SERVO5 PB4
#define SERVO6 PB5
 
 
uint8_t pwm_neutral_programming_mode = 0;
uint8_t eeprom_neutral_pos[6] EEMEM = { 127, 127, 127, 127, 127, 127 };
uint16_t eeprom_pwm_limit[6] EEMEM = { 0, 0, 0, 0, 0, 0 };
uint8_t pwm_signal[6] = { 0, 0, 0, 0, 0, 0 };
 
uint8_t pwm_signal[6];
uint8_t pwm_counter = 0;
uint8_t pwm_neutral_position[6] = { 127, 127, 127, 127, 127, 127 };
uint8_t pwm_position[6] = { 127, 127, 127, 127, 127, 127 };
uint8_t pwm_neutral_position[6];
uint8_t pwm_position[6];
uint16_t pwm_limit[6];
 
 
71,37 → 61,17
 
void eeprom_init() {
 
/*
eeprom_busy_wait();
for(uint8_t i = 0; i < 6; i++) {
pwm_neutral_position[i] = eeprom_read_byte(&eeprom_neutral_pos[i]);
pwm_limit[i] = eeprom_read_byte(&eeprom_pwm_limit[i]);
}
*/
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_neutral_position[0] = 0b10101000;
pwm_neutral_position[1] = 0b01101101;
pwm_neutral_position[2] = 0b10000000;
pwm_neutral_position[3] = 0b10010101;
pwm_neutral_position[4] = 0;
pwm_neutral_position[5] = 127;
 
pwm_limit[0] = 0;
pwm_limit[1] = 0;
pwm_limit[2] = 0;
pwm_limit[3] = 0b1011001101110101;
pwm_limit[4] = 0;
pwm_limit[5] = 0;
 
}
 
void eeprom_write() {
 
cli();
for(uint8_t i = 0; i < 6; i++) {
eeprom_write_byte(&eeprom_neutral_pos[i], pwm_position[i]);
eeprom_write_byte(&eeprom_pwm_limit[i], pwm_limit[i]);
}
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();
 
}
117,12 → 87,6
 
int main(void) {
 
#define PROGRAMMINGMODE 0
#if PROGRAMMINGMODE
uint8_t current_servo = 0;
uint8_t keyrate = 0;
#endif
 
DDRD = 0x80;
PORTD = 0x80;
 
130,22 → 94,13
PORTC |= 0x0f;
 
cli();
uart_init();
eeprom_init();
InitIC2_Slave(0x82);
i2c_init(0x82);
pwm_init();
set_pwm_neutral();
sei();
 
#if PROGRAMMINGMODE
DDRD = 0xff;
PORTD = 0xff;
if (!(PINC & 0x01)) {
pwm_neutral_programming_mode = 1;
PORTD = ~(1 << current_servo);
delay(1000);
}
#endif
 
TCNT1 = 0;
while(1) {
 
188,44 → 143,6
while(TCNT1 < 20000) ;
TCNT1 = 0;
 
#if PROGRAMMINGMODE
 
if (pwm_neutral_programming_mode) {
 
if (keyrate == 0) {
 
if (!(PINC & 0x01)) {
if (pwm_position[current_servo] < 255) {
pwm_position[current_servo]++;
keyrate = 5;
set_pwm();
}
}
if (!(PINC & 0x02)) {
if (pwm_position[current_servo] > 0) {
pwm_position[current_servo]--;
keyrate = 5;
set_pwm();
}
}
if (!(PINC & 0x04)) {
current_servo++;
current_servo %= 6;
PORTD = ~(1 << current_servo);
keyrate = 25;
}
if (!(PINC & 0x08)) {
//keyrate = 50;
PORTD = ~pwm_position[current_servo];
}
 
} else {
keyrate--;
}
 
}
#endif
 
}
 
return 0;
/branches/V0.76g-acid/servoboard/servoboard.h
1,9 → 1,28
 
 
#define VERSION "0.2"
 
 
extern uint8_t pwm_position[6];
extern uint8_t pwm_neutral_programming_mode;
extern uint8_t pwm_neutral_position[6];
extern uint16_t pwm_limit[6];
 
 
void set_pwm();
void set_pwm_neutral();
 
#define GRN_ON PORTD&=~0x01
#define GRN_OFF PORTD|=0x01
void eeprom_init();
void eeprom_write();
 
#define GRN_ON PORTD &= ~0x01
#define GRN_OFF PORTD |= 0x01
 
#define SERVODDR DDRB
#define SERVOPORT PORTB
#define SERVO1 PB0
#define SERVO2 PB1
#define SERVO3 PB2
#define SERVO4 PB3
#define SERVO5 PB4
#define SERVO6 PB5
 
/branches/V0.76g-acid/servoboard/twislave.c
9,7 → 9,7
uint8_t I2C_RXBuffer[32];
uint8_t Byte_Counter = 0;
 
void InitIC2_Slave(uint8_t adr)
void i2c_init(uint8_t adr)
{
TWAR = adr;
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
24,10 → 24,10
return crc;
}
 
ISR (TWI_vect) {
ISR(TWI_vect) {
 
static char cnt = 0;
if (pwm_neutral_programming_mode == 0 && cnt++ == 0) {
if (cnt++ == 0) {
if (PORTD & 0x80) PORTD &= ~0x80; else PORTD |= 0x80;
}
40,7 → 40,7
if (Byte_Counter < 32) {
I2C_RXBuffer[Byte_Counter++] = TWDR;
}
if (Byte_Counter == 7 && pwm_neutral_programming_mode == 0) {
if (Byte_Counter == 7) {
if (calc_crc(&I2C_RXBuffer, 6) == I2C_RXBuffer[6]) {
memcpy(pwm_position, I2C_RXBuffer, 6);
}
48,11 → 48,11
TWCR |= (1<<TWINT);
return;
case TWI_BUS_ERR_2:
TWCR |=(1<<TWSTO) | (1<<TWINT);
TWCR |= (1<<TWSTO) | (1<<TWINT);
case TWI_BUS_ERR_1:
TWCR |=(1<<TWSTO) | (1<<TWINT);
TWCR |= (1<<TWSTO) | (1<<TWINT);
}
TWCR =(1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
TWCR = (1 << TWEA) | (1 << TWINT) | (1 << TWEN) | (1 << TWIE);
 
}
 
/branches/V0.76g-acid/servoboard/twislave.h
1,7 → 1,7
#ifndef _TWI_SLAVE_H_
#define _TWI_SLAVE_H_
 
extern void InitIC2_Slave (uint8_t adr);
extern void i2c_init(uint8_t adr);
 
#define TWI_BUS_ERR_1 0x00
#define TWI_BUS_ERR_2 0xF8
/branches/V0.76g-acid/servoboard/uart.c
0,0 → 1,153
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "servoboard.h"
#include "uart.h"
 
uint8_t rx_buffer[RX_BUFFER_SZ];
//uint8_t tx_buffer[TX_BUFFER_SZ];
uint8_t rx_pos, tx_pos;
 
void uart_init() {
 
rx_pos = 0;
tx_pos = 0;
 
UCR=(1 << TXEN) | (1 << RXEN);
USR |= (1<<U2X);
UCSRB |= (1<<RXCIE);
// UCSRB |= (1<<TXCIE);
 
UBRR = (F_CPU / (BAUD_RATE * 8L) - 1);
 
fdevopen(uart_putchar, NULL);
 
printf("servoboard %s\n> ", VERSION);
 
 
}
 
ISR(USART_RXC_vect) {
 
if (rx_pos < RX_BUFFER_SZ) {
 
uint8_t i = UDR;
uart_putchar(i);
 
if (i == '\r') {
 
uart_putchar('\n');
 
rx_buffer[rx_pos] = 0;
rx_pos = 0;
 
if (!strcasecmp(rx_buffer, "HELP")) {
printf("DISPLAY Display servo settings\n"
"EXPORT Export srvo settings\n"
"SET s=n,l,u Set servo settings\n"
" s = servo number (1-6)\n"
" n = neutral position (0-255)\n"
" l = lower limit (0-255)\n"
" u = upper limit (0-255)\n"
"LOAD Load settings from eeprom\n"
"SAVE Write settings to eeprom\n");
} else if (!strcasecmp(rx_buffer, "DISPLAY")) {
for(i = 0; i < 6; i++) {
printf("SERVO %d, NEUTRAL %d, LIMIT %d-%d\n",
(i + 1),
(pwm_neutral_position[i]),
(pwm_limit[i] & 0xff),
(pwm_limit[i] >> 8)
);
}
} else if (!strcasecmp(rx_buffer, "EXPORT")) {
for(i = 0; i < 6; i++) {
printf("SET %d=%d,%d,%d\n",
(i + 1),
(pwm_neutral_position[i]),
(pwm_limit[i] & 0xff),
(pwm_limit[i] >> 8)
);
}
} else if (!strcasecmp(rx_buffer, "LOAD")) {
eeprom_init();
} else if (!strcasecmp(rx_buffer, "SAVE")) {
eeprom_write();
// } else if (!strncasecmp(rx_buffer, "INC ", 4)) {
// } else if (!strncasecmp(rx_buffer, "DEC ", 4)) {
} else if (!strncasecmp(rx_buffer, "SET ", 4)) {
char *s, *t;
uint8_t servo;
s = strtok_r(rx_buffer, " ", &t);
if (s) {
s = strtok_r(NULL, "=", &t);
if (s) {
servo = atoi(s);
if (servo >= 1 && servo <= 6) {
servo--;
s = strtok_r(NULL, ",", &t);
if (s) {
uint8_t h = 0, l = 0;
pwm_neutral_position[servo] = atoi(s);
s = strtok_r(NULL, ",", &t);
if (s) {
l = atoi(s);
s = strtok_r(NULL, ",", &t);
if (s) {
h = atoi(s);
} else {
h = 0xff;
}
}
pwm_limit[servo] = (h << 8) | l;
i = servo;
printf("SERVO %d, NEUTRAL %d, LIMIT %d-%d\n",
(i + 1),
(pwm_neutral_position[i]),
(pwm_limit[i] & 0xff),
(pwm_limit[i] >> 8)
);
}
} else {
printf("Invalid servo\n");
}
}
}
} else {
printf("Invalid command, type HELP\n");
}
 
uart_putchar('>');
uart_putchar(' ');
 
} else {
 
rx_buffer[rx_pos++] = i;
 
}
 
} else {
 
printf("Receive buffer full.\n");
 
}
 
};
 
/*
ISR(USART_TXC_vect) {
}*/
 
int uart_putchar (char c) {
if (c == '\n') {
uart_putchar('\r');
}
loop_until_bit_is_set(USR, UDRE);
UDR = c;
return 0;
}
 
/branches/V0.76g-acid/servoboard/uart.h
0,0 → 1,14
 
#define BAUD_RATE 9600
 
#define USR UCSRA
#define UCR UCSRB
#define UBRR UBRRL
#define EICR EICRB
#define INT_VEC_TX SIG_UART_TRANS
 
#define RX_BUFFER_SZ 128
#define TX_BUFFER_SZ 128
 
void uart_init();
int uart_putchar (char c);