/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); |