
#include "Arduino.h"
#include <AFMotor.h>

AF_DCMotor moteur1(1);
AF_DCMotor moteur2(3);

float vitesse1;
float vitesse2;
int v1;
int v2;

void Moteurs_Init(){
  // demarrage des moteurs
  moteur1.setSpeed(200);
  moteur2.setSpeed(200);
  moteur1.run(RELEASE);
  moteur2.run(RELEASE);
  moteur1.run(BRAKE);
  moteur2.run(BRAKE);
}

void Moteurs_running(float vitesse, float dir, float coeffDG){
    vitesse1 = (vitesse + dir)*coeffDG;
    vitesse2 = vitesse - dir;
    v1 = (int)(abs(vitesse1));
    if (v1>255) { 
      v1=255; 
      } else if (v1<40) {
        v1=40;
      }
    v2 = (int)(abs(vitesse2)); 
    if (v2>255) { 
      v2=255; 
      } else if (v2<40) {
        v2=40;
      }
    
    if ( vitesse1 > 0) {
      moteur1.run(BACKWARD); 
      } else { 
        moteur1.run(FORWARD); 
        }
    if ( vitesse2 > 0) { 
      moteur2.run(BACKWARD); 
      } else { 
        moteur2.run(FORWARD); 
        }
    moteur1.setSpeed(v1);
    moteur2.setSpeed(v2);
}

void Moteurs_braking() {
  moteur1.setSpeed(0);
  moteur2.setSpeed(0);
  moteur1.run(BRAKE);
  moteur2.run(BRAKE);
}

