Probleme bei Programmierung der Snake Vision für ASURO

[nooby21]

Ich versuche schon seit einiger Zeit meinen ASURO, den ich mit der Snake Vision erweitert habe, dazu zu bringen, dass er auf die wärmste Quelle, die er registriert zufährt, und wenn er an sie stößt ein stückweit rückwärts fahren.Danach sollte er nichts mehr tun. So leicht es sich auch anhört, bin ich mit meinen sehr beschränkten Programmierfähigkeiten nicht weit gekommen.

#include "asuro.h"

void ThermalData(unsigned int *data)
{
	
	ADMUX = (1 << REFS0) | (1 << REFS1) | IR_LEFT;	
		ADCSRA |= (1 << ADSC);    
	while (!(ADCSRA & (1 << ADIF)));	                
		ADCSRA |= (1 << ADIF);			        
	
	data[0] = ADCL + (ADCH << 8);

	
	ADMUX = (1 << REFS0) | (1 << REFS1) | IR_RIGHT;
		ADCSRA |= (1 << ADSC);
		while (!(ADCSRA & (1 << ADIF)));	
		ADCSRA |= (1 << ADIF);
	data[1] = ADCL + (ADCH << 8);
}


int main(void)
{
	
    	unsigned int tdata[2];
		signed int diff,sum;
    	Init();
	
	
	while(1)
    	{
	
        	ThermalData(tdata);
	
		sum=tdata[0]+tdata[1];
		StatusLED(RED);

		if(PollSwitch()==0){
         MotorDir(FWD,FWD);

	
			diff=((signed)tdata[0]-(signed)tdata[1])*32/sum;
			if (diff>4) {
				
				BackLED(ON,OFF); 
				MotorSpeed(0,140);
			}
			else if (diff<-4) {
			
				BackLED(OFF,ON); 
				MotorSpeed(140, 0);
			}
			else {
			
				BackLED(OFF,OFF); 
				MotorSpeed(140, 140);
			}
	}

		else {
			
			StatusLED(GREEN);
			BackLED(OFF,OFF);
			MotorDir(RWD,RWD);
			MotorSpeed(140,140);
			Sleep(2000);

       
		}
    	}     
	return 0;
}

Mit diesem Programm fährt mein ASURO lediglich rechtsrum im Kreis und fährt nur solange rückwärts wie ein Taster gedrückt wird, obwohl er nach Tasterdrück eine gewisse zeitlang rückwärtsfahren soll.

Was muss ich verändern, damit das Programm funktioniert wie ich es will??? Oder ist schon der Ansatz an sich falsch???

Ich freu mich auf jede nur erdenkliche Rückmeldung, da ich selbst langsam nichts mehr verstehe.....

MfG nooby21