Rev 1455 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1455 | Rev 1456 | ||
---|---|---|---|
1 | 1 | ||
2 | #include <string.h> |
2 | #include <string.h> |
3 | #include <avr/io.h> |
3 | #include <avr/io.h> |
4 | #include <util/twi.h> |
4 | #include <util/twi.h> |
5 | #include <avr/interrupt.h> |
5 | #include <avr/interrupt.h> |
6 | #include "servoboard.h" |
6 | #include "servoboard.h" |
7 | #include "twislave.h" |
7 | #include "twislave.h" |
8 | 8 | ||
9 | unsigned char I2C_RXBuffer[32]; |
9 | uint8_t I2C_RXBuffer[32]; |
10 | unsigned char Byte_Counter = 0; |
- | |
11 | unsigned char I2C_Timeout = 0; |
10 | uint8_t Byte_Counter = 0; |
12 | 11 | ||
13 | void InitIC2_Slave(uint8_t adr) |
12 | void InitIC2_Slave(uint8_t adr) |
14 | { |
13 | { |
15 | TWAR = adr; |
14 | TWAR = adr; |
16 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
15 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
17 | } |
16 | } |
18 | 17 | ||
19 | uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
18 | uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
20 | uint8_t crc = 0xff; |
19 | uint8_t crc = 0xff; |
21 | uint8_t i; |
20 | uint8_t i; |
22 | for(i = 0; i < l; i++) { |
21 | for(i = 0; i < l; i++) { |
23 | crc = crc ^ buffer[i]; |
22 | crc = crc ^ buffer[i]; |
24 | } |
23 | } |
25 | return crc; |
24 | return crc; |
26 | } |
25 | } |
27 | 26 | ||
28 | ISR (TWI_vect) { |
27 | ISR (TWI_vect) { |
29 | - | ||
30 | #if 1 |
28 | |
31 | static char cnt=0; |
29 | static char cnt = 0; |
32 | if (pwm_neutral_programming_mode == 0 && cnt++ == 0) { |
30 | if (pwm_neutral_programming_mode == 0 && cnt++ == 0) { |
33 | if (PORTD&0x80) PORTD&=~0x80; else PORTD|=0x80; |
31 | if (PORTD & 0x80) PORTD &= ~0x80; else PORTD |= 0x80; |
34 | } |
- | |
35 | #endif |
32 | } |
36 | - | ||
37 | switch (TWSR & 0xF8) |
33 | |
38 | { |
34 | switch (TWSR & 0xF8) { |
39 | case SR_SLA_ACK: |
35 | case SR_SLA_ACK: |
40 | TWCR |= (1<<TWINT); |
36 | TWCR |= (1<<TWINT); |
41 | Byte_Counter = 0; |
37 | Byte_Counter = 0; |
42 | return; |
38 | return; |
43 | case SR_PREV_ACK: |
39 | case SR_PREV_ACK: |
44 | I2C_Timeout = 250; |
- | |
45 | if (Byte_Counter < 32) { |
40 | if (Byte_Counter < 32) { |
46 | I2C_RXBuffer[Byte_Counter++] = TWDR; |
41 | I2C_RXBuffer[Byte_Counter++] = TWDR; |
47 | } |
42 | } |
48 | if (Byte_Counter == 7 && pwm_neutral_programming_mode == 0) { |
43 | if (Byte_Counter == 7 && pwm_neutral_programming_mode == 0) { |
49 | if (calc_crc(&I2C_RXBuffer, 6) == I2C_RXBuffer[6]) { |
44 | if (calc_crc(&I2C_RXBuffer, 6) == I2C_RXBuffer[6]) { |
50 | memcpy(pwm_position, I2C_RXBuffer, 6); |
45 | memcpy(pwm_position, I2C_RXBuffer, 6); |
51 | } |
46 | } |
52 | } |
47 | } |
53 | TWCR |= (1<<TWINT); |
48 | TWCR |= (1<<TWINT); |
54 | return; |
49 | return; |
55 | case TWI_BUS_ERR_2: |
50 | case TWI_BUS_ERR_2: |
56 | TWCR |=(1<<TWSTO) | (1<<TWINT); |
51 | TWCR |=(1<<TWSTO) | (1<<TWINT); |
57 | case TWI_BUS_ERR_1: |
52 | case TWI_BUS_ERR_1: |
58 | TWCR |=(1<<TWSTO) | (1<<TWINT); |
53 | TWCR |=(1<<TWSTO) | (1<<TWINT); |
59 | } |
54 | } |
60 | TWCR =(1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE); // TWI Reset |
55 | TWCR =(1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
- | 56 | ||
61 | } |
57 | } |
62 | 58 | ||
63 | 59 | ||
64 | 60 |