Блог

Я пытаюсь решить задачу, следующую за линией, с помощью робота 3Pi , используя 3 ИК-датчика (массив в моем коде), и хотя я заставляю свой массив работать, получаю правильные данные на последовательном мониторе и устанавливаю пороговое значение для своих датчиков, я, похоже, не могу заставить робота следовать линии : робот движется вперед, но игнорирует линию.

Я добавляю свой код здесь:

 const int left_sensor_pin = A0;
const int right_sensor_pin = A3;
const int centre_sensor_pin = A2;

// store values into array
#define NB_LS_PINS 3
int ls_pin[NB_LS_PINS] = { left_sensor_pin, centre_sensor_pin, right_sensor_pin };
int which;

// store time
unsigned long start_time;
unsigned long elapsed_time [3];
unsigned long end_time_ls [3];
unsigned long set_motors;

bool done = false;

# define L_PWM_PIN 10
# define L_DIR_PIN 16
# define R_PWM_PIN 9
# define R_DIR_PIN 15

void setup() {
  Serial.begin (9600);
  pinMode (left_sensor_pin, INPUT);
  pinMode (right_sensor_pin, INPUT);
  pinMode (centre_sensor_pin, INPUT);
  delay(5000);

  pinMode(L_PWM_PIN, OUTPUT);
  pinMode(L_DIR_PIN, OUTPUT);
  pinMode(R_PWM_PIN, OUTPUT);
  pinMode(R_DIR_PIN, OUTPUT);

  digitalWrite(L_DIR_PIN, LOW);
  digitalWrite(R_DIR_PIN, LOW);

  analogWrite(L_PWM_PIN, 50);
  analogWrite(R_PWM_PIN, 50);
}

void loop() {

  //charge capacitor
  pinMode (left_sensor_pin, OUTPUT);
  pinMode (right_sensor_pin, OUTPUT);
  pinMode (centre_sensor_pin, OUTPUT);
  digitalWrite(left_sensor_pin, HIGH);
  digitalWrite(right_sensor_pin, HIGH);
  digitalWrite(centre_sensor_pin, HIGH);

  // Delay for capacitor to charge.
  delayMicroseconds(10);
  start_time = micros();

  pinMode (left_sensor_pin, INPUT);
  pinMode (right_sensor_pin, INPUT);
  pinMode (centre_sensor_pin, INPUT);

  bool pins_read [3] = {false, false, false};
  done = false;

  while ( done == false ) {

    for ( int which = 0; which < NB_LS_PINS; which  ) {

      if ( digitalRead (ls_pin[ which ]) == LOW amp;amp; pins_read [which] == false) { // need also to track whether pins are read, a single = is setting to a value and == compares values

        end_time_ls [which] = micros ();
        elapsed_time [which] = micros() - start_time;
        pins_read [which] = true;
      }

      if (pins_read [0] == true amp;amp; pins_read [1] == true amp;amp; pins_read [2] == true) {

        done = true;
      }
    }
  }

  elapsed_time[0] = end_time_ls [0] - start_time;
  elapsed_time[1] = end_time_ls [1] - start_time;
  elapsed_time[2] = end_time_ls [2] - start_time;

  Serial.println (elapsed_time [0]);
  Serial.println (elapsed_time [1]);
  Serial.println (elapsed_time [2]);
  Serial.println("------------------------------");

  //Motors

  if (elapsed_time [0] < 10000 amp;amp; elapsed_time[1] > 10000 amp;amp; elapsed_time[2]  < 10000)
  {
    //go forward
    analogWrite( L_PWM_PIN, 100 );
    analogWrite( R_PWM_PIN, 100 );
  }

  else if (elapsed_time [0] > 10000 amp;amp; elapsed_time[1] < 10000 amp;amp; elapsed_time[2]  < 10000)
  {
    // Turn right.
    analogWrite( L_PWM_PIN, 100 );
    analogWrite( R_PWM_PIN, 0 );
  }
  else if (elapsed_time [0] < 10000 amp;amp; elapsed_time[1] < 10000 amp;amp; elapsed_time[2]  > 10000)
  {
    // turn left.
    analogWrite( L_PWM_PIN, 0 );
    analogWrite( R_PWM_PIN, 100 );
  }
}