RobotExplorerContubot1

#define pinLED 13

#define pinRX 0
#define pinTX 1

// MOTOR LEFT
// power
#define pinWheelL_PWMA 5
// dir
#define pinWheelL_AIN1 8
#define pinWheelL_AIN2 9

// MOTOR RIGHT
// power
#define pinWheelR_PWMB 6
// dir
#define pinWheelR_BIN2 11
#define pinWheelR_BIN1 12

// ENCODER
#define pinWheelL_ENCD 2
#define pinWheelR_ENCD 3

// ULTRASOUND
#define pinUS_TRIG 10
#define pinUS_ECHO 4


#define pinWheelStandby 7

#define MAXPOWER 255
#define STEPS_L 5
#define STEPS_R 5
#define STEPS_B 10
#define STEPS_F 20

// A01,A02 // LEFT MOTOR
// B01,B02 // LEFT MOTOR

// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
const int Nt=2;
unsigned long t0_ms[Nt];        // will store last time LED was updated

// constants won't change :
long dt_ms[Nt];           // interval at which to do something

// ROBOT MODE

#define EXPLORER 0 // explorer
#define LFOLLOWER 1 // explorer

int robotMode=EXPLORER;

#define MAXSPEED 255
int speedR=MAXSPEED;
int speedL=MAXSPEED;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(57600);

  pinMode(LED_BUILTIN, OUTPUT); 

  pinMode(pinWheelL_AIN1, OUTPUT); 
  pinMode(pinWheelL_AIN2, OUTPUT); 
  
  pinMode(pinWheelR_BIN1, OUTPUT); 
  pinMode(pinWheelR_BIN2, OUTPUT);   

// SONAR
  pinMode(pinUS_TRIG, OUTPUT); 
  pinMode(pinUS_ECHO, INPUT);
//
  pinMode(pinWheelStandby, OUTPUT);   
  digitalWrite(pinWheelStandby, HIGH);  
  
  dt_ms[0] = 10;           // interval at which to avareage angular velocity
 //Serial.println("> Ready!");
}

void loop() {

  unsigned long t_ms = millis();
  //
  // check against collision system
  if (t_ms - t0_ms[2] >= dt_ms[2]) {
    // save the last time you did it
    t0_ms[0] = t_ms;
   int d=ultrasound();
   
         setMotor(255,255);
         
    if (d<10) {
      Serial.println(d);   

    }
  }  
}


// Function to Spin Right
void setMotor(int speedL, int speedR) {
  int speedLabs=abs(speedL);
  int speedRabs=abs(speedR);

  if (speedL>0) {
    digitalWrite(pinWheelL_AIN1, LOW);
    digitalWrite(pinWheelL_AIN2, HIGH);
  }

  if (speedL<0) {
    digitalWrite(pinWheelL_AIN1, HIGH);
    digitalWrite(pinWheelL_AIN2, LOW);
  }


  if (speedR>0) {
    digitalWrite(pinWheelR_BIN1, LOW);
    digitalWrite(pinWheelR_BIN2, HIGH);
  }

  if (speedR<0) {
    digitalWrite(pinWheelR_BIN1, HIGH);
    digitalWrite(pinWheelR_BIN2, LOW);
  }

  
  // set power
  analogWrite(pinWheelL_PWMA, speedLabs);
  analogWrite(pinWheelR_PWMB, speedRabs);
}

long ultrasound() {
 digitalWrite(pinUS_TRIG, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(pinUS_TRIG, HIGH);
//  delayMicroseconds(1000); - Removed this line
  delayMicroseconds(10); // Added this line
  digitalWrite(pinUS_TRIG, LOW);
  long duration_us = pulseIn(pinUS_ECHO, HIGH);
  long distance_cm = (duration_us/2) / 29.1;

  if (distance_cm<200) {
    Serial.println(distance_cm);
  }
    //delay(10);
    return(distance_cm);
}