| 0,0 → 1,63 |
|---|
| |
| #include <string.h> |
| #include <avr/io.h> |
| #include <util/twi.h> |
| #include <avr/interrupt.h> |
| #include "servoboard.h" |
| #include "twislave.h" |
| |
| unsigned char I2C_RXBuffer[32]; |
| unsigned char Byte_Counter = 0; |
| unsigned char I2C_Timeout = 0; |
| |
| void InitIC2_Slave(uint8_t adr) |
| { |
| TWAR = adr; |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
| } |
| |
| uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
| uint8_t crc = 0xff; |
| uint8_t i; |
| for(i = 0; i < l; i++) { |
| crc = crc ^ buffer[i]; |
| } |
| return crc; |
| } |
| |
| ISR (TWI_vect) { |
| |
| #if 1 |
| static char cnt=0; |
| if (pwm_neutral_programming_mode == 0 && cnt++ == 0) { |
| if (PORTD&0x80) PORTD&=~0x80; else PORTD|=0x80; |
| } |
| #endif |
| |
| switch (TWSR & 0xF8) |
| { |
| case SR_SLA_ACK: |
| TWCR |= (1<<TWINT); |
| Byte_Counter = 0; |
| return; |
| case SR_PREV_ACK: |
| I2C_Timeout = 250; |
| if (Byte_Counter < 32) { |
| I2C_RXBuffer[Byte_Counter++] = TWDR; |
| } |
| if (Byte_Counter == 7 && pwm_neutral_programming_mode == 0) { |
| if (calc_crc(&I2C_RXBuffer, 6) == I2C_RXBuffer[6]) { |
| memcpy(pwm_position, I2C_RXBuffer, 6); |
| } |
| } |
| TWCR |= (1<<TWINT); |
| return; |
| case TWI_BUS_ERR_2: |
| TWCR |=(1<<TWSTO) | (1<<TWINT); |
| case TWI_BUS_ERR_1: |
| TWCR |=(1<<TWSTO) | (1<<TWINT); |
| } |
| TWCR =(1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE); // TWI Reset |
| } |
| |
| |