From 6a8060625fd11c8c0c9ae35f527016f3cf0e2bae Mon Sep 17 00:00:00 2001
From: pfrison <pierre.frison@polytech-lille.net>
Date: Sat, 30 Mar 2019 15:40:23 +0100
Subject: [PATCH] PercTeacher test robot concluant

---
 PercTeacher/Sources/Robot/servoAndStepperControl.c | 42 ++++++++++++++++++++++--------------------
 1 file changed, 22 insertions(+), 20 deletions(-)

diff --git a/PercTeacher/Sources/Robot/servoAndStepperControl.c b/PercTeacher/Sources/Robot/servoAndStepperControl.c
index 88a4299..8828c19 100644
--- a/PercTeacher/Sources/Robot/servoAndStepperControl.c
+++ b/PercTeacher/Sources/Robot/servoAndStepperControl.c
@@ -2,7 +2,7 @@
 #include <util/delay.h>
 
 #define SERIAL_BAUDE_RATE 9600
-#define DELAI_STEP 1000
+#define DELAI_STEP 2
 #define PIN_MOTEUR_GAUCHE 0x40 // pin 12
 #define PIN_MOTEUR_DROIT 0x20 // pin 11
 
@@ -10,24 +10,20 @@
 
 //gestion des moteurs
 void init_pins(void){
-    // pin 12 et 11
     DDRB = PIN_MOTEUR_GAUCHE | PIN_MOTEUR_DROIT;
 }
 
-void pas_moteur_gauche(void){
-    // pin 12
-    PORTB |= PIN_MOTEUR_GAUCHE;
-    _delay_us(DELAI_STEP);
-    PORTB &= ~PIN_MOTEUR_GAUCHE;
-    _delay_us(DELAI_STEP);
-}
-
-void pas_moteur_droit(void){
-    // pin 11
-    PORTB |= PIN_MOTEUR_DROIT;
-    _delay_us(DELAI_STEP);
-    PORTB &= ~PIN_MOTEUR_DROIT;
-    _delay_us(DELAI_STEP);
+void pas_moteur(int g, int d){
+    if (g)
+        PORTB |= PIN_MOTEUR_GAUCHE;
+    if (d)
+        PORTB |= PIN_MOTEUR_DROIT;
+    _delay_ms(DELAI_STEP);
+    if (g)
+        PORTB &= ~PIN_MOTEUR_GAUCHE;
+    if (d)
+        PORTB &= ~PIN_MOTEUR_DROIT;
+    _delay_ms(DELAI_STEP);
 }
 
 //gestion de la liaison serie
@@ -57,6 +53,9 @@ int main(void){
         unsigned char deltaL2 = get_serial();
         unsigned char deltaR1 = get_serial();
         unsigned char deltaR2 = get_serial();
+
+
+	send_serial('a');
         
         // conversion 2x8 bits => 16 bits
         int deltaL = (deltaL1 << 8) + deltaL2;
@@ -64,12 +63,15 @@ int main(void){
         
         int deltaMax = deltaL > deltaR ? deltaL : deltaR;
         for(int i=0; i<deltaMax; i++){
-            if(deltaL > i)
-                pas_moteur_gauche();
-            if(deltaR > i)
-                pas_moteur_droit();
+            if(deltaL > i && deltaR > i)
+                pas_moteur(1, 1);
+            else if(deltaL > i)
+                pas_moteur(1, 0);
+            else if(deltaR > i)
+                pas_moteur(0, 1);
         }
 	
+	send_serial('b');
 	send_serial(0x01);
     }
     return 0;
--
libgit2 0.21.2