Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1184 | pangu | 1 | /*############################################################################ |
| 2 | ############################################################################*/ |
||
| 3 | |||
| 4 | #include "main.h" |
||
| 5 | |||
| 6 | volatile unsigned char twi_state = 0; |
||
| 7 | unsigned char motor = 0; |
||
| 8 | unsigned char motorread = 0; |
||
| 9 | unsigned char motor_rx[16]; |
||
| 10 | |||
| 11 | //############################################################################ |
||
| 12 | //Initzialisieren der I2C (TWI) Schnittstelle |
||
| 13 | void i2c_init(void) |
||
| 14 | //############################################################################ |
||
| 15 | { |
||
| 16 | TWSR = 0; |
||
| 17 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
||
| 18 | } |
||
| 19 | |||
| 20 | //############################################################################ |
||
| 21 | //Start I2C |
||
| 22 | char i2c_start(void) |
||
| 23 | //############################################################################ |
||
| 24 | { |
||
| 25 | TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
||
| 26 | return(0); |
||
| 27 | } |
||
| 28 | |||
| 29 | //############################################################################ |
||
| 30 | void i2c_stop(void) |
||
| 31 | //############################################################################ |
||
| 32 | { |
||
| 33 | TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
||
| 34 | } |
||
| 35 | |||
| 36 | void i2c_reset(void) |
||
| 37 | //############################################################################ |
||
| 38 | { |
||
| 39 | i2c_stop(); |
||
| 40 | twi_state = 0; |
||
| 41 | motor = TWDR; |
||
| 42 | motor = 0; |
||
| 43 | TWCR = 0x80; |
||
| 44 | TWAMR = 0; |
||
| 45 | TWAR = 0; |
||
| 46 | TWDR = 0; |
||
| 47 | TWSR = 0; |
||
| 48 | TWBR = 0; |
||
| 49 | i2c_init(); |
||
| 50 | i2c_start(); |
||
| 51 | i2c_write_byte(0); |
||
| 52 | } |
||
| 53 | |||
| 54 | //############################################################################ |
||
| 55 | char i2c_write_byte(char byte) |
||
| 56 | //############################################################################ |
||
| 57 | { |
||
| 58 | TWSR = 0x00; |
||
| 59 | TWDR = byte; |
||
| 60 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
||
| 61 | |||
| 62 | return(0); |
||
| 63 | |||
| 64 | } |
||
| 65 | |||
| 66 | |||
| 67 | #ifdef QUADRO |
||
| 68 | //############################################################################ |
||
| 69 | SIGNAL (TWI_vect) |
||
| 70 | //############################################################################ |
||
| 71 | { |
||
| 72 | switch (twi_state++) |
||
| 73 | { |
||
| 74 | case 0: |
||
| 75 | i2c_write_byte(0x52+(motor*2)); |
||
| 76 | break; |
||
| 77 | case 1: |
||
| 78 | switch(motor++) |
||
| 79 | { |
||
| 80 | case 0: |
||
| 81 | i2c_write_byte(Motor_Vorne); |
||
| 82 | break; |
||
| 83 | case 1: |
||
| 84 | i2c_write_byte(Motor_Hinten); |
||
| 85 | break; |
||
| 86 | case 2: |
||
| 87 | i2c_write_byte(Motor_Rechts); |
||
| 88 | break; |
||
| 89 | case 3: |
||
| 90 | i2c_write_byte(Motor_Links); |
||
| 91 | break; |
||
| 92 | } |
||
| 93 | break; |
||
| 94 | case 2: |
||
| 95 | i2c_stop(); |
||
| 96 | if (motor<4) twi_state = 0; |
||
| 97 | else motor = 0; |
||
| 98 | i2c_start(); |
||
| 99 | break; |
||
| 100 | |||
| 101 | //Liest Daten von Motor |
||
| 102 | case 3: |
||
| 103 | i2c_write_byte(0x53+(motorread*2)); |
||
| 104 | break; |
||
| 105 | case 4: |
||
| 106 | switch(motorread) |
||
| 107 | { |
||
| 108 | case 0: |
||
| 109 | i2c_write_byte(Motor_Vorne); |
||
| 110 | break; |
||
| 111 | case 1: |
||
| 112 | i2c_write_byte(Motor_Hinten); |
||
| 113 | break; |
||
| 114 | case 2: |
||
| 115 | i2c_write_byte(Motor_Rechts); |
||
| 116 | break; |
||
| 117 | case 3: |
||
| 118 | i2c_write_byte(Motor_Links); |
||
| 119 | break; |
||
| 120 | } |
||
| 121 | break; |
||
| 122 | case 5: //1 Byte vom Motor lesen |
||
| 123 | motor_rx[motorread] = TWDR; |
||
| 124 | |||
| 125 | case 6: |
||
| 126 | switch(motorread) |
||
| 127 | { |
||
| 128 | case 0: |
||
| 129 | i2c_write_byte(Motor_Vorne); |
||
| 130 | break; |
||
| 131 | case 1: |
||
| 132 | i2c_write_byte(Motor_Hinten); |
||
| 133 | break; |
||
| 134 | case 2: |
||
| 135 | i2c_write_byte(Motor_Rechts); |
||
| 136 | break; |
||
| 137 | case 3: |
||
| 138 | i2c_write_byte(Motor_Links); |
||
| 139 | break; |
||
| 140 | } |
||
| 141 | break; |
||
| 142 | case 7: //2 Byte vom Motor lesen |
||
| 143 | motor_rx[motorread+4] = TWDR; |
||
| 144 | motorread++; |
||
| 145 | if (motorread>3) motorread=0; |
||
| 146 | i2c_stop(); |
||
| 147 | I2CTimeout = 10; |
||
| 148 | twi_state = 0; |
||
| 149 | break; |
||
| 150 | case 8: // Gyro-Offset |
||
| 151 | i2c_write_byte(0x98); // Address of the DAC |
||
| 152 | break; |
||
| 153 | case 9: |
||
| 154 | i2c_write_byte(0x10); // Update Channel A |
||
| 155 | break; |
||
| 156 | case 10: |
||
| 157 | i2c_write_byte(AnalogOffsetNick); // Value |
||
| 158 | break; |
||
| 159 | case 11: |
||
| 160 | i2c_write_byte(0x80); // Value |
||
| 161 | break; |
||
| 162 | case 12: |
||
| 163 | i2c_stop(); |
||
| 164 | I2CTimeout = 10; |
||
| 165 | i2c_start(); |
||
| 166 | break; |
||
| 167 | case 13: |
||
| 168 | i2c_write_byte(0x98); // Address of the DAC |
||
| 169 | break; |
||
| 170 | case 14: |
||
| 171 | i2c_write_byte(0x12); // Update Channel B |
||
| 172 | break; |
||
| 173 | case 15: |
||
| 174 | i2c_write_byte(AnalogOffsetRoll); // Value |
||
| 175 | break; |
||
| 176 | case 16: |
||
| 177 | i2c_write_byte(0x80); // Value |
||
| 178 | break; |
||
| 179 | case 17: |
||
| 180 | i2c_stop(); |
||
| 181 | I2CTimeout = 10; |
||
| 182 | i2c_start(); |
||
| 183 | break; |
||
| 184 | case 18: |
||
| 185 | i2c_write_byte(0x98); // Address of the DAC |
||
| 186 | break; |
||
| 187 | case 19: |
||
| 188 | i2c_write_byte(0x14); // Update Channel C |
||
| 189 | break; |
||
| 190 | case 20: |
||
| 191 | i2c_write_byte(AnalogOffsetGier); // Value |
||
| 192 | break; |
||
| 193 | case 21: |
||
| 194 | i2c_write_byte(0x80); // Value |
||
| 195 | break; |
||
| 196 | case 22: |
||
| 197 | i2c_stop(); |
||
| 198 | I2CTimeout = 10; |
||
| 199 | twi_state = 0; |
||
| 200 | break; |
||
| 201 | } |
||
| 202 | TWCR |= 0x80; |
||
| 203 | } |
||
| 204 | #endif |
||
| 205 | #if defined(HEXA1) || defined(HEXA2) |
||
| 206 | //############################################################################ |
||
| 207 | SIGNAL (TWI_vect) |
||
| 208 | //############################################################################ |
||
| 209 | { |
||
| 210 | switch (twi_state++) |
||
| 211 | { |
||
| 212 | case 0: |
||
| 213 | i2c_write_byte(0x52+(motor*2)); |
||
| 214 | break; |
||
| 215 | case 1: |
||
| 216 | switch(motor++) |
||
| 217 | { |
||
| 218 | case 0: |
||
| 219 | i2c_write_byte(Motor1); |
||
| 220 | break; |
||
| 221 | case 1: |
||
| 222 | i2c_write_byte(Motor2); |
||
| 223 | break; |
||
| 224 | case 2: |
||
| 225 | i2c_write_byte(Motor3); |
||
| 226 | break; |
||
| 227 | case 3: |
||
| 228 | i2c_write_byte(Motor4); |
||
| 229 | break; |
||
| 230 | case 4: |
||
| 231 | i2c_write_byte(Motor5); |
||
| 232 | break; |
||
| 233 | case 5: |
||
| 234 | i2c_write_byte(Motor6); |
||
| 235 | break; |
||
| 236 | } |
||
| 237 | break; |
||
| 238 | case 2: |
||
| 239 | i2c_stop(); |
||
| 240 | if (motor<6) twi_state = 0; |
||
| 241 | else motor = 0; |
||
| 242 | i2c_start(); |
||
| 243 | break; |
||
| 244 | |||
| 245 | //Liest Daten von Motor |
||
| 246 | case 3: |
||
| 247 | i2c_write_byte(0x53+(motorread*2)); |
||
| 248 | break; |
||
| 249 | case 4: |
||
| 250 | switch(motorread) |
||
| 251 | { |
||
| 252 | case 0: |
||
| 253 | i2c_write_byte(Motor1); |
||
| 254 | break; |
||
| 255 | case 1: |
||
| 256 | i2c_write_byte(Motor2); |
||
| 257 | break; |
||
| 258 | case 2: |
||
| 259 | i2c_write_byte(Motor3); |
||
| 260 | break; |
||
| 261 | case 3: |
||
| 262 | i2c_write_byte(Motor4); |
||
| 263 | break; |
||
| 264 | case 4: |
||
| 265 | i2c_write_byte(Motor5); |
||
| 266 | break; |
||
| 267 | case 5: |
||
| 268 | i2c_write_byte(Motor6); |
||
| 269 | break; |
||
| 270 | } |
||
| 271 | break; |
||
| 272 | case 5: //1 Byte vom Motor lesen |
||
| 273 | motor_rx[motorread] = TWDR; |
||
| 274 | |||
| 275 | case 6: |
||
| 276 | switch(motorread) |
||
| 277 | { |
||
| 278 | case 0: |
||
| 279 | i2c_write_byte(Motor1); |
||
| 280 | break; |
||
| 281 | case 1: |
||
| 282 | i2c_write_byte(Motor2); |
||
| 283 | break; |
||
| 284 | case 2: |
||
| 285 | i2c_write_byte(Motor3); |
||
| 286 | break; |
||
| 287 | case 3: |
||
| 288 | i2c_write_byte(Motor4); |
||
| 289 | break; |
||
| 290 | case 4: |
||
| 291 | i2c_write_byte(Motor5); |
||
| 292 | break; |
||
| 293 | case 5: |
||
| 294 | i2c_write_byte(Motor6); |
||
| 295 | break; |
||
| 296 | } |
||
| 297 | break; |
||
| 298 | case 7: //2 Byte vom Motor lesen |
||
| 299 | motor_rx[motorread+6] = TWDR; |
||
| 300 | motorread++; |
||
| 301 | if (motorread>5) motorread=0; |
||
| 302 | i2c_stop(); |
||
| 303 | I2CTimeout = 10; |
||
| 304 | twi_state = 0; |
||
| 305 | break; |
||
| 306 | case 8: // Gyro-Offset |
||
| 307 | i2c_write_byte(0x98); // Address of the DAC |
||
| 308 | break; |
||
| 309 | case 9: |
||
| 310 | i2c_write_byte(0x10); // Update Channel A |
||
| 311 | break; |
||
| 312 | case 10: |
||
| 313 | i2c_write_byte(AnalogOffsetNick); // Value |
||
| 314 | break; |
||
| 315 | case 11: |
||
| 316 | i2c_write_byte(0x80); // Value |
||
| 317 | break; |
||
| 318 | case 12: |
||
| 319 | i2c_stop(); |
||
| 320 | I2CTimeout = 10; |
||
| 321 | i2c_start(); |
||
| 322 | break; |
||
| 323 | case 13: |
||
| 324 | i2c_write_byte(0x98); // Address of the DAC |
||
| 325 | break; |
||
| 326 | case 14: |
||
| 327 | i2c_write_byte(0x12); // Update Channel B |
||
| 328 | break; |
||
| 329 | case 15: |
||
| 330 | i2c_write_byte(AnalogOffsetRoll); // Value |
||
| 331 | break; |
||
| 332 | case 16: |
||
| 333 | i2c_write_byte(0x80); // Value |
||
| 334 | break; |
||
| 335 | case 17: |
||
| 336 | i2c_stop(); |
||
| 337 | I2CTimeout = 10; |
||
| 338 | i2c_start(); |
||
| 339 | break; |
||
| 340 | case 18: |
||
| 341 | i2c_write_byte(0x98); // Address of the DAC |
||
| 342 | break; |
||
| 343 | case 19: |
||
| 344 | i2c_write_byte(0x14); // Update Channel C |
||
| 345 | break; |
||
| 346 | case 20: |
||
| 347 | i2c_write_byte(AnalogOffsetGier); // Value |
||
| 348 | break; |
||
| 349 | case 21: |
||
| 350 | i2c_write_byte(0x80); // Value |
||
| 351 | break; |
||
| 352 | case 22: |
||
| 353 | i2c_stop(); |
||
| 354 | I2CTimeout = 10; |
||
| 355 | twi_state = 0; |
||
| 356 | break; |
||
| 357 | } |
||
| 358 | TWCR |= 0x80; |
||
| 359 | } |
||
| 360 | |||
| 361 | #endif |
||
| 362 | |||
| 363 | #if defined(OCTO) || defined(OCTO2) || defined (OCTO3) |
||
| 364 | //############################################################################ |
||
| 365 | SIGNAL (TWI_vect) |
||
| 366 | //############################################################################ |
||
| 367 | { |
||
| 368 | switch (twi_state++) |
||
| 369 | { |
||
| 370 | case 0: |
||
| 371 | i2c_write_byte(0x52+(motor*2)); |
||
| 372 | break; |
||
| 373 | case 1: |
||
| 374 | switch(motor++) |
||
| 375 | { |
||
| 376 | case 0: |
||
| 377 | i2c_write_byte(Motor1); |
||
| 378 | break; |
||
| 379 | case 1: |
||
| 380 | i2c_write_byte(Motor2); |
||
| 381 | break; |
||
| 382 | case 2: |
||
| 383 | i2c_write_byte(Motor3); |
||
| 384 | break; |
||
| 385 | case 3: |
||
| 386 | i2c_write_byte(Motor4); |
||
| 387 | break; |
||
| 388 | case 4: |
||
| 389 | i2c_write_byte(Motor5); |
||
| 390 | break; |
||
| 391 | case 5: |
||
| 392 | i2c_write_byte(Motor6); |
||
| 393 | break; |
||
| 394 | case 6: |
||
| 395 | i2c_write_byte(Motor7); |
||
| 396 | break; |
||
| 397 | case 7: |
||
| 398 | i2c_write_byte(Motor8); |
||
| 399 | break; |
||
| 400 | } |
||
| 401 | break; |
||
| 402 | case 2: |
||
| 403 | i2c_stop(); |
||
| 404 | if (motor<8) twi_state = 0; |
||
| 405 | else motor = 0; |
||
| 406 | i2c_start(); |
||
| 407 | break; |
||
| 408 | |||
| 409 | //Liest Daten von Motor |
||
| 410 | case 3: |
||
| 411 | i2c_write_byte(0x53+(motorread*2)); |
||
| 412 | break; |
||
| 413 | case 4: |
||
| 414 | switch(motorread) |
||
| 415 | { |
||
| 416 | case 0: |
||
| 417 | i2c_write_byte(Motor1); |
||
| 418 | break; |
||
| 419 | case 1: |
||
| 420 | i2c_write_byte(Motor2); |
||
| 421 | break; |
||
| 422 | case 2: |
||
| 423 | i2c_write_byte(Motor3); |
||
| 424 | break; |
||
| 425 | case 3: |
||
| 426 | i2c_write_byte(Motor4); |
||
| 427 | break; |
||
| 428 | case 4: |
||
| 429 | i2c_write_byte(Motor5); |
||
| 430 | break; |
||
| 431 | case 5: |
||
| 432 | i2c_write_byte(Motor6); |
||
| 433 | break; |
||
| 434 | case 6: |
||
| 435 | i2c_write_byte(Motor7); |
||
| 436 | break; |
||
| 437 | case 7: |
||
| 438 | i2c_write_byte(Motor8); |
||
| 439 | break; |
||
| 440 | } |
||
| 441 | break; |
||
| 442 | case 5: //1 Byte vom Motor lesen |
||
| 443 | motor_rx[motorread] = TWDR; |
||
| 444 | |||
| 445 | case 6: |
||
| 446 | switch(motorread) |
||
| 447 | { |
||
| 448 | case 0: |
||
| 449 | i2c_write_byte(Motor1); |
||
| 450 | break; |
||
| 451 | case 1: |
||
| 452 | i2c_write_byte(Motor2); |
||
| 453 | break; |
||
| 454 | case 2: |
||
| 455 | i2c_write_byte(Motor3); |
||
| 456 | break; |
||
| 457 | case 3: |
||
| 458 | i2c_write_byte(Motor4); |
||
| 459 | break; |
||
| 460 | case 4: |
||
| 461 | i2c_write_byte(Motor5); |
||
| 462 | break; |
||
| 463 | case 5: |
||
| 464 | i2c_write_byte(Motor6); |
||
| 465 | break; |
||
| 466 | case 6: |
||
| 467 | i2c_write_byte(Motor7); |
||
| 468 | break; |
||
| 469 | case 7: |
||
| 470 | i2c_write_byte(Motor8); |
||
| 471 | break; |
||
| 472 | } |
||
| 473 | break; |
||
| 474 | case 7: //2 Byte vom Motor lesen |
||
| 475 | motor_rx[motorread+8] = TWDR; |
||
| 476 | motorread++; |
||
| 477 | if (motorread>7) motorread=0; |
||
| 478 | i2c_stop(); |
||
| 479 | I2CTimeout = 10; |
||
| 480 | twi_state = 0; |
||
| 481 | break; |
||
| 482 | case 8: // Gyro-Offset |
||
| 483 | i2c_write_byte(0x98); // Address of the DAC |
||
| 484 | break; |
||
| 485 | case 9: |
||
| 486 | i2c_write_byte(0x10); // Update Channel A |
||
| 487 | break; |
||
| 488 | case 10: |
||
| 489 | i2c_write_byte(AnalogOffsetNick); // Value |
||
| 490 | break; |
||
| 491 | case 11: |
||
| 492 | i2c_write_byte(0x80); // Value |
||
| 493 | break; |
||
| 494 | case 12: |
||
| 495 | i2c_stop(); |
||
| 496 | I2CTimeout = 10; |
||
| 497 | i2c_start(); |
||
| 498 | break; |
||
| 499 | case 13: |
||
| 500 | i2c_write_byte(0x98); // Address of the DAC |
||
| 501 | break; |
||
| 502 | case 14: |
||
| 503 | i2c_write_byte(0x12); // Update Channel B |
||
| 504 | break; |
||
| 505 | case 15: |
||
| 506 | i2c_write_byte(AnalogOffsetRoll); // Value |
||
| 507 | break; |
||
| 508 | case 16: |
||
| 509 | i2c_write_byte(0x80); // Value |
||
| 510 | break; |
||
| 511 | case 17: |
||
| 512 | i2c_stop(); |
||
| 513 | I2CTimeout = 10; |
||
| 514 | i2c_start(); |
||
| 515 | break; |
||
| 516 | case 18: |
||
| 517 | i2c_write_byte(0x98); // Address of the DAC |
||
| 518 | break; |
||
| 519 | case 19: |
||
| 520 | i2c_write_byte(0x14); // Update Channel C |
||
| 521 | break; |
||
| 522 | case 20: |
||
| 523 | i2c_write_byte(AnalogOffsetGier); // Value |
||
| 524 | break; |
||
| 525 | case 21: |
||
| 526 | i2c_write_byte(0x80); // Value |
||
| 527 | break; |
||
| 528 | case 22: |
||
| 529 | i2c_stop(); |
||
| 530 | I2CTimeout = 10; |
||
| 531 | twi_state = 0; |
||
| 532 | break; |
||
| 533 | } |
||
| 534 | TWCR |= 0x80; |
||
| 535 | } |
||
| 536 | |||
| 537 | #endif |