DofOriol : Evite_Obstacle_C++

//************************************************
//Créateurs : Oriol Alexandre et Doffemont Florian
//Date : 22/01/20
// Nom : Evite_Obstacle_C++
//************************************************


#include <wiringPi.h>
#include <iostream>
#include <softPwm.h>

using namespace std;

#define PWMA 22
#define AIN1 26
#define AIN2 23

#define PWMB 25
#define BIN1 28
#define BIN2 29

#define IR_OBS_D 24
#define IR_OBS_G 27

int main(){

    wiringPiSetup();

    pinMode(IR_OBS_D, INPUT);
	pullUpDnControl(IR_OBS_D,PUD_UP);
	pinMode(IR_OBS_G, INPUT);
	pullUpDnControl(IR_OBS_G,PUD_UP);

    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
    softPwmCreate(PWMA, 0, 100);

    pinMode(BIN1, OUTPUT);
    pinMode(BIN2, OUTPUT);
    digitalWrite(BIN1, HIGH);
    digitalWrite(BIN2, LOW);
    softPwmCreate(PWMB, 0, 100);

  bool a = digitalRead(IR_OBS_D);
  bool b = digitalRead(IR_OBS_G);

    for(;;){
       if(a == 0 or b == 0){

           digitalWrite(AIN1, HIGH);
           digitalWrite(AIN2, LOW);
           digitalWrite(BIN1, LOW);
           digitalWrite(BIN2, HIGH);

           softPwmWrite(PWMA, 100);
           softPwmWrite(PWMB, 100);
           a = digitalRead(IR_OBS_D);
           b = digitalRead(IR_OBS_G);
           break;
       }
       else {

           digitalWrite(AIN1, HIGH);
           digitalWrite(AIN2, LOW);
           digitalWrite(BIN1, HIGH);
           digitalWrite(BIN2, LOW);

           softPwmWrite(PWMA, 100);
           softPwmWrite(PWMB, 100);
           a = digitalRead(IR_OBS_D);
           b = digitalRead(IR_OBS_G);
           break;
       }
    }
    return 0;
}