Chenillard en assembleur

Bonjour,
je dois réaliser un chenillard qui fonctionne avec un switch, la particularité de ce chenillard est que le nombre de led est commander par un switch mais ça on s'n ofu pour le moment, j'ai pas choisie d'ajouter lea partie des switch mais elle marche .J'appelerai ces mode modechenillard1,2,3 et 4led...

voici mon programme
/*
This example is designed for the REva board with the STM8 daughter board.
It is the simplest possible example for toggling the REva LEDs.
Use it to get to know the REva, STM8, RLink, RIDE, etc.
*/

#include <intrins.h>
#include <STM8\stm8af61xx.h>

#define myGPIO GPIOB

//definition of the port registers
#define PX_ODR ((myGPIO)->ODR)
#define PX_IDR ((myGPIO)->IDR)
#define PX_DDR ((myGPIO)->DDR)
#define PX_CR1 ((myGPIO)->CR1)
#define PX_CR2 ((myGPIO)->CR2)

#define myGPIO2 GPIOG

//definition of the port registers
#define PX2_ODR ((myGPIO2)->ODR)
#define PX2_IDR ((myGPIO2)->IDR)
#define PX2_DDR ((myGPIO2)->DDR)
#define PX2_CR1 ((myGPIO2)->CR1)
#define PX2_CR2 ((myGPIO2)->CR2)

// Choisir lequel des chenillard on utilise

#define MODE_CHENILLARD1Led (u8) //(0xFE)
#define MODE_CHENILLARD2Led (u8) //(0xFC)
#define MODE_CHENILLARD3Led (u8) //(0xF8)
#define MODE_CHENILLARD4Led (u8) //(0xF0)

// Définis le nombnre de led du chenillard selon la valeur du switch
unsigned char TestMode()
{
if (PX2_IDR=0x80)
return MODE_CHENILLARD1led;
else ((PX2_IDR=0x40)
return MODE_CHENILLARD2led;
else ((PX2_IDR=0x20)
return MODE_CHENILLARD3led;
else ((PX2_IDR=0x10)
return MODE_CHENILLARD4led;
return 0;
}

void Chenillard()
{
if(i == 0x00)
i = 0x01;
PX_ODR = ~i;
i <<= 1;
// if(i > 7)
// i = 0

}

//delay function, waiting for a defined period
void delay(unsigned int cnt)
{
//decrement counter until it is zero
while(cnt--)
_nop_();
}

//Program entry point
void main()
{
//wait for some time
//the value of the parameter of this call defines the speed of the toggling
delay(20000);

//main variable, copied on the LEDs
unsigned char i = 0;

//configure PB in output push-pull
PX_DDR = 0xFF;
PX_CR1 = 0xFF;
PX_CR2 = 0x00;

//configure PG in input pull up without interrupt
PX2_DDR = 0x00;
PX2_CR1 = 0xFF;
PX2_CR2 = 0x00;

//main loop
while(1)
{

//Permet de choisir le nombre de led du chenillard
switch (TestMode())
{
case MODE_CHENILLARD1Led :
Chenillard();
break;
case MODE_CHENILLARD2Led:
autrechose();
break;
case MODE_CHENILLARD3Led :
Chenillard();
break;
case MODE_CHENILLARD4Led :
Chenillard();
break;

}

}
}