Rare fout stk500 met interrupt handler

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <util/twi.h>
#include <util/delay.h> 

#define FOSC 8000000								/* Clock Speed */
#define BAUD 9600									/* Baud rate */
#define MYUBRR FOSC/16/BAUD-1
#define TRUE 1
#define FALSE 0
#define HOOGTE 240
#define KP 210
#define KI 220
#define KD 230


unsigned char 	gewensteHoogte  = 0,
				gemetenHoogte	= 0,
				Kp				= 1,
				Ki				= 1,
				Kd				= 1,
				err				= 0,
				verbinding		= 0,
				inData			= 0,
				uitData			= 0,
				softwarenr		= 0,
				vastgetal		= 0,
				msb				= 0,
				lsb				= 0,
				Error 			= 0,
				previous_error 	= 0,
				werkelijkeHoogte = 0,
				derivative = 0,
				dt = 0.0015,
				integral = 0,
				PWM = 0,
				output;

int 			TWI_sensor = 0xF2;


void status(int status){
	//PORTA = 0xff - status;
}

void status1(int status){
	PORTA = 0xff - status;
}

//////////////INI RS232////////////////
void init_RS232(){
	DDRA = 0xFF;
	unsigned char baudrate = MYUBRR;	
	UBRRH 	= 	(baudrate>>8);			/* Set baud rate */
	UBRRL 	= 	baudrate;
	UCSRB 	= 	(1<<RXCIE)|(1<<TXCIE)|(1<<RXEN)|(1<<TXEN);							/* Enable receiver, transmitter and receiver interrupt*/
	UCSRC 	=	(1<<URSEL)|      					/* write to UCSRC */
 				(0<<UMSEL)|     					/* asynchrone */
				(0<<UCSZ2)|(1<<UCSZ1)|(1<<UCSZ0)|  	/* 8 databits */
				(0<<UPM1)|(0<<UPM0)|    			/* no parity */
				(1<<USBS);      					/* 2 stopbits */
	set_sleep_mode(SLEEP_MODE_IDLE);
}


unsigned char receive( void ){
	/* Get and return received data from buffer */
	while ( !(UCSRA & (1<<RXC)) );
	return UDR;
}


void transmit( unsigned char data1 ){
	/* wit for transmitbuffer to empty */
	while ( !( UCSRA & (1<<UDRE)) );
	/* Put data into buffer, sends the data */
	UDR = data1;
}
////////////////INIT timer//////////////////////////////////
void initTimer(){


TIMSK = (1<<OCIE2);
	OCR2  = 255;
	TCCR2 = (0<<WGM20)|(0<<COM20)|(0<<COM21)|(0<<WGM21)|(1<<CS20)|(1<<CS21)|(1<<CS22);
	TCNT2 = 0;
	sei();

	}
//////////////COMMUNICATIE MET DE COMPUTER////////////////////////////////

void ERROR(unsigned char errorCode){
	err 	= ( 1 << errorCode );
}




void SetPWMOutput(uint8_t duty){
   OCR0=duty;
}


void setHoogte(){
	unsigned char tmp;
	tmp = receive();
	if(tmp>0){
	gewensteHoogte = tmp;
	}
}

void setKp(){
	Kp = receive();
	status(60);
}

void setKi(){
	Ki = receive();
	status(70);
}

void setKd(){
	Kd = receive();
	status(80);
}

////////////////////TWI////////////////////
void init_TWI(){
	TWSR = (0<<TWPS1) | (0<<TWPS0); // pre scaler (staat op 0)
	TWBR = 0x32; // init scl  (op 100 kHz)
   	TWCR = (1<<TWEN);
}


void error(){
	while(1){}
}

//Zend de start bit.
void start(){
	TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
	
}
void wait(){
		while (!(TWCR & (1<<TWINT)));
}

void check(unsigned char code){
	if ((TWSR & 0xF8) != code){
		error();
	}
}

void sendAdres(int i){
	if( i == 0 ){
	TWDR = 0xF2;/// write
	}
	if( i == 1 ){
	TWDR = 0xF3;/////read
	}
	TWCR = (1<<TWINT) | (1<<TWEN);
}

void stop(){
	TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN);
	while(TWCR & (1<<TWSTO));
}

void mack(){
    TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
}

void nack(){
	TWCR = (1<<TWINT)|(1<<TWEN);
}

void loadCommand(unsigned char command){
	TWDR = command;	
	TWCR = (1<<TWINT) | (1<<TWEN);
}

int loadValue(){
	return TWDR;
}

void wacht(){
	for ( int i=0; i< 16000; i++){}
}

void set_Gain(){
	start();
	wait();
	check(TW_START); //kijken of start goed overisgekomen
	status(1);
	
	sendAdres(0);
	wait();
	check(TW_MT_SLA_ACK);
	status(18);
	
	loadCommand(0x01);
	wait();
	check(TW_MT_DATA_ACK);
	status(7);
	
	loadCommand(0x00);
	wait();
	check(TW_MT_DATA_ACK);
	status(6);
	
	stop();
	status(9);
}

void set_Range(){
		start();
		wait();
		check(TW_START); //kijken of start goed overisgekomen
		status(1);

		sendAdres(0);
		wait();
		check(TW_MT_SLA_ACK);
		status(18);
				
		loadCommand(0x00);
		wait();
		check(TW_MT_DATA_ACK);
		status(7);

		loadCommand(0x2D);
		wait();
		check(TW_MT_DATA_ACK);
		status(6);

		stop();
		status(9);
}

void  TWI_read_distance(){
		start();
		wait();
		check(TW_START); //kijken of start goed overisgekomen
		status(1);

		sendAdres(0);//write
		wait();
		check(TW_MT_SLA_ACK); // kijken of slave adress goed is gegaan
		status(5);
		
		loadCommand(0x00);
		wait();
		check(TW_MT_DATA_ACK);
		status(6);

		loadCommand(0x51);
		wait();
		check(TW_MT_DATA_ACK);
		status(6);
			
		stop();
		status(9);

		_delay_ms(125);
		start();
		wait();
		check(TW_START); //kijken of start goed overisgekomen
		status(1);
		

		sendAdres(0);
		wait();
		check(TW_MT_SLA_ACK);
		status(18);
				
		loadCommand(0x00);
		wait();
		check(TW_MT_DATA_ACK);
		status(6);

		start();
		wait();
		check(TW_REP_START);
		status(15);

		sendAdres(1);
		wait();
		check(TW_MR_SLA_ACK);
		status(18);
		


		mack();
		wait();
		softwarenr = loadValue();
		check(TW_MR_DATA_ACK);
		status(19);
		
		mack();
		wait();
		vastgetal = loadValue();
		check(TW_MR_DATA_ACK);
		status(19);



		mack();
		wait();
		msb = loadValue();
		check(TW_MR_DATA_ACK);
		status(19);
		
		nack();
		wait();
		lsb = loadValue();
	
		check(TW_MR_DATA_NACK);
		status(23);
		
		//lsb + nack
		
		status(24);
		stop();
		status(28);
		wacht();
}

void PID(){
	Error 			= gewensteHoogte - werkelijkeHoogte;
	integral 		= integral + (Error*dt);
	derivative 		= (Error - previous_error)/dt;
	output 			= (Kp*Error) + (Ki*integral) + (Kd*derivative);
	previous_error 	= Error;
	PWM = ((output*-0.1)+97); 
	
	if(PWM>255){
		PWM = 255;
	}
	if(PWM<0){
		PWM = 0;
	}
	
	SetPWMOutput(PWM);
}

////////PWM/////////////////////////////

void initPWM(){
	TCCR0=0x7B;
	DDRB|=(1<<PB3);
}

///////////Interrupts/////////////////
ISR( USART_RXC_vect ){
	cli();
	switch( inData = receive() ){
		case HOOGTE 	: 	setHoogte(); 		break;
		case KP			: 	setKp(); 			break;
		case KI			: 	setKi(); 			break;
		case KD			: 	setKd(); 			break;
		default	 		:   status(3);
	}
	sei();
}

ISR( TIMER2_COMP_vect ){
	cli();
	TWI_read_distance();
	//werkelijkeHoogte = (200 - lsb) ;
	
	//status1(gewensteHoogte);
	//PID();
	//transmit(output);
	//transmit(werkelijkeHoogte);
	//transmit(PWM);
	//	status1(inData);
	transmit(gewensteHoogte);
	sei();
	}

///////Main/////////////////////////
int main(){
	init_TWI();
	status(1);
	init_RS232();
	status(2);
	set_Gain();
	set_Range();
	initPWM();
	status(3);
	initTimer();
	while(1) { 
		transmit(gewensteHoogte);
		status1(gewensteHoogte);
		
			
	}
	return 1;
}