Rev 1400 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1400 | Rev 1452 | ||
---|---|---|---|
Line 89... | Line 89... | ||
89 | void I2C_ReceiveLastByte(void) |
89 | void I2C_ReceiveLastByte(void) |
90 | { |
90 | { |
91 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
91 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
92 | } |
92 | } |
Line -... | Line 93... | ||
- | 93 | ||
- | 94 | uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
|
- | 95 | uint8_t crc = 0xff; |
|
- | 96 | uint8_t i; |
|
- | 97 | for(i = 0; i < l; i++) { |
|
- | 98 | crc = crc ^ buffer[i]; |
|
- | 99 | } |
|
93 | 100 | return crc; |
|
Line 94... | Line 101... | ||
94 | 101 | } |
|
95 | 102 | ||
96 | //############################################################################ |
103 | //############################################################################ |
97 | SIGNAL (TWI_vect) |
104 | SIGNAL (TWI_vect) |
98 | //############################################################################ |
105 | //############################################################################ |
99 | { |
106 | { |
100 | static unsigned char missing_motor; |
107 | static unsigned char missing_motor; |
101 | switch(twi_state++) |
108 | switch(twi_state++) |
102 | { |
- | |
103 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
104 | // Writing the Data |
- | |
105 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
106 | case 0: |
- | |
107 | while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used |
- | |
108 | if(motor == MAX_MOTORS) // writing finished -> now read |
- | |
109 | { |
- | |
110 | motor = 0; |
- | |
111 | twi_state = 3; |
- | |
112 | i2c_write_byte(0x53+(motorread*2)); |
- | |
113 | } |
- | |
114 | else i2c_write_byte(0x52+(motor*2)); |
- | |
115 | break; |
- | |
116 | case 1: |
- | |
117 | i2c_write_byte(Motor[motor++]); |
- | |
118 | break; |
- | |
119 | case 2: |
- | |
120 | if(TWSR == 0x30) |
- | |
121 | { |
- | |
122 | if(!missing_motor) missing_motor = motor; |
- | |
123 | if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255; |
- | |
124 | } |
- | |
125 | i2c_stop(); |
- | |
126 | I2CTimeout = 10; |
- | |
127 | twi_state = 0; |
- | |
128 | i2c_start(); |
- | |
129 | break; |
- | |
130 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
131 | // Reading Data |
- | |
132 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
133 | case 3: |
- | |
134 | //Transmit 1st byte for reading |
- | |
135 | if(TWSR != 0x40) // Error? |
- | |
136 | { |
- | |
137 | MotorPresent[motorread] = 0; |
- | |
138 | motorread++; |
- | |
139 | if(motorread >= MAX_MOTORS) motorread = 0; |
- | |
140 | i2c_stop(); |
- | |
141 | twi_state = 0; |
- | |
142 | } |
- | |
143 | else |
- | |
144 | { |
- | |
145 | MotorPresent[motorread] = ('1' - '-') + motorread; |
- | |
146 | I2C_ReceiveByte(); |
- | |
147 | } |
- | |
148 | MissingMotor = missing_motor; |
- | |
149 | missing_motor = 0; |
- | |
150 | break; |
- | |
151 | case 4: //Read 1st byte and transmit 2nd Byte |
- | |
152 | motor_rx[motorread] = TWDR; |
- | |
153 | I2C_ReceiveLastByte(); //nack |
- | |
154 | break; |
- | |
155 | case 5: |
- | |
156 | //Read 2nd byte |
- | |
157 | motor_rx2[motorread++] = TWDR; |
- | |
158 | if(motorread >= MAX_MOTORS) motorread = 0; |
- | |
159 | i2c_stop(); |
- | |
160 | twi_state = 0; |
- | |
161 | break; |
- | |
162 | 109 | { |
|
163 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
110 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
164 | // writing Gyro-Offset |
111 | // writing Gyro-Offset |
165 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
112 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
166 | case 8: |
113 | case 8: |
Line 212... | Line 159... | ||
212 | case 22: |
159 | case 22: |
213 | i2c_stop(); |
160 | i2c_stop(); |
214 | I2CTimeout = 10; |
161 | I2CTimeout = 10; |
215 | twi_state = 0; |
162 | twi_state = 0; |
216 | break; |
163 | break; |
- | 164 | ||
- | 165 | case 0: |
|
- | 166 | i2c_write_byte(0x82); // servo board |
|
- | 167 | twi_state = 100; |
|
- | 168 | break; |
|
- | 169 | case 100: // servo 1 |
|
- | 170 | i2c_write_byte(servoValues[0]); |
|
- | 171 | break; |
|
- | 172 | case 101: // servo 2 |
|
- | 173 | i2c_write_byte(servoValues[1]); |
|
- | 174 | break; |
|
- | 175 | case 102: // servo 3 |
|
- | 176 | i2c_write_byte(servoValues[2]); |
|
- | 177 | break; |
|
- | 178 | case 103: // servo 4 |
|
- | 179 | i2c_write_byte(servoValues[3]); |
|
- | 180 | break; |
|
- | 181 | case 104: // servo 5 |
|
- | 182 | i2c_write_byte(servoValues[4]); |
|
- | 183 | break; |
|
- | 184 | case 105: // servo 6 |
|
- | 185 | i2c_write_byte(servoValues[5]); |
|
- | 186 | break; |
|
- | 187 | case 106: |
|
- | 188 | i2c_write_byte(calc_crc(&servoValues, 6)); |
|
- | 189 | break; |
|
- | 190 | case 107: |
|
- | 191 | i2c_stop(); |
|
- | 192 | I2CTimeout = 10; |
|
- | 193 | twi_state = 0; |
|
- | 194 | break; |
|
217 | default: twi_state = 0; |
195 | default: twi_state = 0; |
218 | break; |
196 | break; |
219 | } |
197 | } |
220 | TWCR |= 0x80; |
198 | TWCR |= 0x80; |
221 | } |
199 | } |