diff --git a/PercTeacher/Sources/Robot/servoAndStepperControl.c b/PercTeacher/Sources/Robot/servoAndStepperControl.c
index 5231d23..88a4299 100644
--- a/PercTeacher/Sources/Robot/servoAndStepperControl.c
+++ b/PercTeacher/Sources/Robot/servoAndStepperControl.c
@@ -6,6 +6,8 @@
 #define PIN_MOTEUR_GAUCHE 0x40 // pin 12
 #define PIN_MOTEUR_DROIT 0x20 // pin 11
 
+#define CPU_FREQ 16000000L
+
 //gestion des moteurs
 void init_pins(void){
     // pin 12 et 11
@@ -57,8 +59,8 @@ int main(void){
         unsigned char deltaR2 = get_serial();
         
         // conversion 2x8 bits => 16 bits
-        int deltaL = deltaL1 << 8 + deltaL2;
-        int deltaR = deltaR1 << 8 + deltaR2;
+        int deltaL = (deltaL1 << 8) + deltaL2;
+        int deltaR = (deltaR1 << 8) + deltaR2;
         
         int deltaMax = deltaL > deltaR ? deltaL : deltaR;
         for(int i=0; i<deltaMax; i++){
@@ -67,7 +69,8 @@ int main(void){
             if(deltaR > i)
                 pas_moteur_droit();
         }
-        
+	
+	send_serial(0x01);
     }
     return 0;
 }
diff --git a/tests/atmega2560/servoAndStepperControl/servoAndStepperControl.c b/tests/atmega2560/servoAndStepperControl/servoAndStepperControl.c
index 746fe1f..6a4bf4b 100644
--- a/tests/atmega2560/servoAndStepperControl/servoAndStepperControl.c
+++ b/tests/atmega2560/servoAndStepperControl/servoAndStepperControl.c
@@ -3,13 +3,27 @@
 
 int main(void){
 	// pin 13
-	DDRB = 0x80;
+	DDRB = 0xC0;
 
+	PORTB |= 0x40; // forward
+	int i = 0;
+	int sens = 0;
 	while(1){
-		PORTB = 0x80;
-		_delay_us(1000);
-		PORTB = 0x00;
-		_delay_us(1000);
+		// step
+		PORTB |= 0x80;
+		_delay_ms(5);
+		PORTB &= 0x7F;
+		_delay_ms(5);
+		// dir
+		i++;
+		if(i > 200){
+			i = 0;
+			sens = 1 - sens;
+			if(sens == 0)
+				PORTB |= 0x40; // forward
+			else
+				PORTB &= 0xBF; // backward
+		}
 	}
 	return 0;
 }
--
libgit2 0.21.2