From 3fcb1d7bf4f287e3324e2ea669c1bc7c64077fe6 Mon Sep 17 00:00:00 2001
From: pfrison <pierre.frison@polytech-lille.net>
Date: Thu, 11 Apr 2019 18:10:32 +0200
Subject: [PATCH] update robot code for PercTeacher

---
 PercTeacher/Sources/Robot/robot.c | 58 ++++++++++++++++++++++++++++++++++++----------------------
 1 file changed, 36 insertions(+), 22 deletions(-)

diff --git a/PercTeacher/Sources/Robot/robot.c b/PercTeacher/Sources/Robot/robot.c
index 8828c19..737deed 100644
--- a/PercTeacher/Sources/Robot/robot.c
+++ b/PercTeacher/Sources/Robot/robot.c
@@ -3,29 +3,42 @@
 
 #define SERIAL_BAUDE_RATE 9600
 #define DELAI_STEP 2
-#define PIN_MOTEUR_GAUCHE 0x40 // pin 12
-#define PIN_MOTEUR_DROIT 0x20 // pin 11
+#define STEP_MOTEUR_GAUCHE 0x00 // pin ??
+#define STEP_MOTEUR_DROIT 0x00 // pin ??
+#define DIR_MOTEUR_GAUCHE 0x00 // pin ??
+#define DIR_MOTEUR_DROIT 0x00 // pin ??
 
 #define CPU_FREQ 16000000L
 
 //gestion des moteurs
 void init_pins(void){
-    DDRB = PIN_MOTEUR_GAUCHE | PIN_MOTEUR_DROIT;
+    DDRB = STEP_MOTEUR_GAUCHE | STEP_MOTEUR_DROIT | DIR_MOTEUR_GAUCHE | DIR_MOTEUR_DROIT;
 }
 
 void pas_moteur(int g, int d){
-    if (g)
-        PORTB |= PIN_MOTEUR_GAUCHE;
-    if (d)
-        PORTB |= PIN_MOTEUR_DROIT;
+    if (g != 0)
+        PORTB |= STEP_MOTEUR_GAUCHE;
+    if (d != 0)
+        PORTB |= STEP_MOTEUR_DROIT;
     _delay_ms(DELAI_STEP);
-    if (g)
-        PORTB &= ~PIN_MOTEUR_GAUCHE;
-    if (d)
-        PORTB &= ~PIN_MOTEUR_DROIT;
+    if (g != 0)
+        PORTB &= ~STEP_MOTEUR_GAUCHE;
+    if (d != 0)
+        PORTB &= ~STEP_MOTEUR_DROIT;
     _delay_ms(DELAI_STEP);
 }
 
+void moteur_set_dir(int g, int d){
+	if(g != 0)
+        PORTB |= DIR_MOTEUR_GAUCHE;
+	else
+        PORTB &= ~DIR_MOTEUR_GAUCHE;
+	if(d != 0)
+        PORTB |= DIR_MOTEUR_DROIT;
+	else
+        PORTB &= ~DIR_MOTEUR_DROIT;
+}
+
 //gestion de la liaison serie
 void init_serial(int speed){
     UBRR0 = CPU_FREQ / (((unsigned long int) speed) << 4) - 1; //Set baud rate
@@ -53,16 +66,18 @@ 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;
-        int deltaR = (deltaR1 << 8) + deltaR2;
+        uint16_t deltaL = (deltaL1 << 8) + deltaL2;
+        uint16_t deltaR = (deltaR1 << 8) + deltaR2;
+		
+		// sens rotation
+		moteur_set_dir((deltaL & 0x80) == 0x80 ? 1 : 0, (deltaR & 0x80) == 0x80 ? 1 : 0)
+		deltaL &= 0x7F;
+		deltaR &= 0x7F;
         
-        int deltaMax = deltaL > deltaR ? deltaL : deltaR;
-        for(int i=0; i<deltaMax; i++){
+        uint16_t deltaMax = deltaL > deltaR ? deltaL : deltaR;
+        for(uint16_t i=0; i<deltaMax; i++){
             if(deltaL > i && deltaR > i)
                 pas_moteur(1, 1);
             else if(deltaL > i)
@@ -70,9 +85,8 @@ int main(void){
             else if(deltaR > i)
                 pas_moteur(0, 1);
         }
-	
-	send_serial('b');
-	send_serial(0x01);
+		
+		send_serial(0x01);
     }
     return 0;
 }
--
libgit2 0.21.2