Créer un Algorithme

Bonjour,
Je doit créer cet algorithme pour un TP d'électronique , mais le Problème , c'est je n'y arrive pas c'est très important
MERCI BEAUCOUP:
Voici le programme :


#include<p18f4520.h>
#include<timers.h>

void initialisations(void)
{

OSCCONbits.SCS0=0;
OSCCONbits.SCS1=1; //oscillateur interne
OSCCONbits.IRCF0=1; //sélection de la fréquence
OSCCONbits.IRCF1=1; //de l'horloge interne
OSCCONbits.IRCF2=1; //à 8MHz
//OSCTUNEbits.PLLEN=1; //4x8MHz=32MHz
while(OSCCONbits.IOFS==0);


// lignes bumpers RD0(droit), RD1(gauche)
// lignes wheel drop RD2(gauche), RD3(droit), RD4(avant)
// lignes Boutons Poussoirs BPP1/RD5(PLAY), BP2/RD6(ADVANCE) et RB5(/BP)
TRISD=0xFF;

//diodes LED1 RE0
// LED2 RE1
// LED2 RE2
TRISE=0;

//OSC RB0/INT0
//défaut moteur droit RB1/INT1
//défaut moteur gauche RB2/INT2
// /BP
TRISB=0xFF;

//ligne sens moteur gauche RA6
//ligne sens moteur droit RA7
TRISAbits.TRISA6=0;
TRISAbits.TRISA7=0;
//ligne PWM moteur gauche
//ligne PWM moteur droit
TRISCbits.TRISC1=0;
TRISCbits.TRISC2=0;
//initialisation PWM pour une fréquence de 1000Hz avec rapport cyclique nul
PR2=132; //pour fosc interne à 8MHz avec facteur prédivision pour le Timer2 à 1 => fpwm=15000Hz
CCPR1L=0; //rapport cyclique 0%
CCPR2L=0; //rapport cyclique 0%
CCP1CON=0x0C; //P1M1 et P1M0 à 0 / DC1B1 et DC1B0 à 0 / CCP1M3=1 et CCP1M2=1
CCP2CON=0x0C; //DC2B1 et DC2B0 à 0 / CCP2M3=1 et CCP2M2=1
T2CONbits.T2CKPS0=0;
T2CONbits.T2CKPS1=0; //prescaler timer2 à 1
T2CONbits.TMR2ON=1;

//convertisseur AN CLIFF et WALL ADCON1: -- -- VCFG1 VCFG0 PCFG3 PCFG2 PCFG1 PCFG0
// ADCON2: ADFM -- ACQT2 ACQT1 ACQT0 ADCS2 ADCS1 ADCS0
ADCON1=0b00001010; //VREF-=Vss VREF+=Vdd AN0 à AN4 entrées analogiques
ADCON2=0b10010010; //justification droite 4TAD 8Tosc
ADCON0bits.ADON=1;
TRISAbits.TRISA0=1; //CLIFF1 RA0/AN0
TRISAbits.TRISA1=1; //CLIFF2 RA1/AN1
TRISAbits.TRISA2=1; //CLIFF3 RA2/AN2
TRISAbits.TRISA3=1; //CLIFF4 RA3/AN3
TRISAbits.TRISA5=1; //WALL RA5/AN4


//I2C pour fscl=100KHz avec fosc=8MHz
//data format : Start condition
// envoi adresse 01001010 adresse ave R/W à 0 et A0 à 1 (par câblage)
// envoi donnée xxD5D4D3D2D1D0 ou 0
// Stop condition
SSPADD=19;
SSPCON1=0b00101000;
SSPSTATbits.SMP=1;


/*********INTERRUPTIONS*********/
//Un seul niveau de priorité : HAUTE
RCONbits.IPEN=0;

//osc sur INT0
//réglage pour détecter front montant
TRISBbits.TRISB0=1;
INTCONbits.INT0IF=0;
INTCON2bits.INTEDG0=1;
INTCONbits.INT0IF=0;

//réglage Timer3 (sert à fixer la durée d'une note)
T3CON=0b10110000;
PIR2bits.TMR3IF=0;
IPR2bits.TMR3IP=1;

//réglage interruption sur les lignes RB4 à RB7
INTCONbits.RBIF=0;
INTCON2bits.RBIP=1;

//réglage Timer0 pour CODEUR DROIT
T0CON=0b00101000;
INTCONbits.TMR0IF=0;
INTCON2bits.TMR0IP=1;

//validation individuelle des différentes sources d'interruptions
INTCONbits.INT0IE=1; //OSC
INTCONbits.TMR0IE=1; //codeur droit
PIE2bits.TMR3IE=1; //son
INTCONbits.RBIE=1; //boutons poussoirs
INTCONbits.PEIE=1; //validation interruptions périphériques
}