AVR Temp über RGB LED

#define     F_CPU       3686400
#include <avr/io.h>
#include <avr/interrupt.h>

// Temperatursensor
#define     TEMP_ADDR   0x10010001 // Adresse, A1 = A2 = A3 = 0, Lesemodus

// TWI
#define     WAIT_TWI()  loop_until_bit_is_set(TWCR, TWINT);
#define     F_SCL       100000
#define     TWI_PRESC   1
#define     TWBR_VAL    (F_CPU / F_SCL - 16) / (2 * TWI_PRESC)

#if TWBR_VAL > 0xff
#   error "TWBR ist zu gross, bitte einen anderen TWI-Prescaler setzen"
#elif TWBR_VAL < 0
#   undef TWBRVAL
#   define TWBR_VAL 0
#endif

// Ausgänge
#define     RED_ON()        PORTB |= (1 << 0);
#define     GREEN_ON()  PORTB |= (1 << 1);
#define     BLUE_ON()   PORTB |= (1 << 2);
#define     RED_OFF()   PORTB &= ~(1 << 0);
#define     GREEN_OFF() PORTB &= ~(1 << 1);
#define     BLUE_OFF()  PORTB &= ~(1 << 2);

volatile uint8_t R = 1, G = 1, B = 1; // RGB-Farbraum global

// I2C Ansteuerung
void twi_init (void)
{
    TWBR = TWBR_VAL;
}

void twi_start (unsigned char adress)
{
    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // Start
    WAIT_TWI()
    TWDR = adress;  // Adresse eingeben
    TWCR = (1 << TWINT) | (1 << TWEN); // Senden
    WAIT_TWI()
}

void twi_stop (void)
{
    TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // Stopp
}

unsigned char twi_readAck (void) // Empfangen mit ACK
{
    unsigned char data;

    TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); // Empfangen mit ACK
    WAIT_TWI()
    data = TWDR;
    return data;
}

unsigned char twi_readNak (void) // Empfangen mit NAK
{
    unsigned char data;

    TWCR = (1 << TWINT) | (1 << TWEN); // Empfangen mit NAK
    WAIT_TWI()
    data = TWDR;
    return data;
}

int16_t readLM75 (uint8_t adress)
{
    uint8_t ret, msg;

    twi_start(adress);
    msg = twi_readAck();
    ret = msg << 8;
    msg = twi_readNak();
    ret |= msg;
    twi_stop();

    return ret;
}

// Interrupt Timer 0 (PWM)
ISR (TIMER0_OVF_vect)
{
    static uint8_t pwm;

    if (R == pwm)
        RED_OFF()

    if (G == pwm)
        GREEN_OFF()

    if (B == pwm)
        BLUE_OFF()

    pwm++;
    if (pwm == 255) {
        pwm = 0;
        RED_ON()
        GREEN_ON()
        BLUE_ON()
    }
}

// Interrupt Timer 1 (Temperatur holen und RGB berechnen)
ISR (TIMER1_OVF_vect)
{
    int16_t lm75_temp;
    int8_t temp = 1;
    uint16_t hue, sat, bri, red_val, green_val, blue_val;

    // Temperatur holen
    lm75_temp = readLM75(TEMP_ADDR);
    temp = lm75_temp >> 8;
    lm75_temp >>= 7;

    // Temperatur umrechnen in gewünschten Farbwert
    hue = temp * 25;
    sat = 255;
    bri = 255;

    // Umrechnung HSB zu RGB
    while (hue > 1529) hue -= 1530;
    while (hue < 0) hue += 1530;

    if (hue < 255) {
       red_val = 255;
       green_val = (65280 - sat * (255 - hue)) >> 8;
       blue_val = 255 - sat;
    }
    else if (hue < 510) {
       red_val = (65280 - sat * (hue - 255)) >> 8;
       green_val = 255;
       blue_val = 255 - sat;
    }
    else if (hue < 765) {
       red_val = 255 - sat;
       green_val = 255;
       blue_val = (65280 - sat * (765 - hue)) >> 8;
    }
    else if (hue < 1020) {
       red_val = 255 - sat;
       green_val = (65280 - sat * (hue - 765)) >> 8;
       blue_val = 255;
    }
    else if (hue < 1275) {
       red_val = (65280 - sat * (1275 - hue)) >> 8;
       green_val = 255 - sat;
       blue_val = 255;
    }
    else {
       red_val = 255;
       green_val = 255 - sat;
       blue_val = (65280 - sat * (hue - 1275)) >> 8;
    }

    R = ((bri + 1) * red_val) >> 8;
    G = ((bri + 1) * green_val) >> 8;
    B = ((bri + 1) * blue_val) >> 8;
}

// Hauptfunktion
int main (void)
{
    // Ausgänge bestimmen
    DDRB = (1 << 0) | (1 << 1) | (1 << 2);
    DDRC = (1 << 3) | (1 << 4); // Two Wire Interface

    // 8 bit Timer
    TCCR0 = (1 << CS00); // Prescaler aus
    TIMSK |= (1 << TOIE0); // Overflow Interrupt erlauben

    // 16 bit Timer
    TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0)); // Output-Pin nicht ansteuern
    TCCR1A &= ~((1 << WGM10) | (1 << WGM11)); // PWM Modus aus
    TCCR1B |= (1 << CS12) | (1 << CS10); // Prescaler 1024
    TIMSK |= (1 << TOIE1); // Overflow Interrupt erlauben

    // TWI Initialisieren
    twi_init();

    // Global Interrupts erlauben
    sei();

    while (1) {
        // Nichts tun
    }

    return 0;
}