3.4 Line follow - Arduino

Sensorul de line follow

4 cazuri posibile

cele 4 cazuri

Schema logică a programului

Schema logică a programului

O versiune a codului final (linia albă)

#include "MeMCore.h"

MeLineFollower lineFinder(PORT_2); /* Line Finder module can only be connected to PORT_3, PORT_4, PORT_5, PORT_6 of base shield. */

MeDCMotor motor1(M1);
MeDCMotor motor2(M2);

float MOTOR1_TUNE = -1.0;
float MOTOR2_TUNE = 1.0;

uint8_t motorSpeed = 100;

bool prev_move_left = false;
bool prev_move_fwd = true;

void setup()
{
  // Optional: robotul porneste de la un buton
  pinMode(7, INPUT); //Define button pin as input
  while (analogRead(7) > 100) {
    delay(50); //Wait till button pressed to start.
  }
 
  // setez comunicarea prin portul serial
  Serial.begin(9600);
}

void activateLeftMotor() {
   motor1.run(MOTOR1_TUNE*motorSpeed); /* value: between -255 and 255. */
}

void activateRightMotor() {
   motor2.run(MOTOR2_TUNE*motorSpeed); /* value: between -255 and 255. */
}

void stopLeftMotor() {
   motor1.stop();
}

void stopRightMotor() {
   motor2.stop();
}

void turnLeft() {
  activateLeftMotor();
  stopRightMotor();
}

void turnRight() {
  activateRightMotor();
  stopLeftMotor();
}


void bothMotorsRun() {
  activateRightMotor();
  activateLeftMotor();
 }

void loop()
{
  int sensorState1 = lineFinder.readSensors();

  int sensorState = 1;
  
  if(sensorState1==S1_IN_S2_IN) {
    sensorState = S1_OUT_S2_OUT;
  }
  else if(sensorState1==S1_IN_S2_OUT) {
    sensorState = S1_OUT_S2_IN;
  }
  else if(sensorState1==S1_OUT_S2_IN) {
    sensorState = S1_IN_S2_OUT;
  }
  else if(sensorState1==S1_OUT_S2_OUT) {
    sensorState = S1_IN_S2_IN;
  }
  
  switch(sensorState)
  {
    case S1_IN_S2_IN: 
      Serial.println("Sensor 1 and 2 are inside of black line"); 
      bothMotorsRun();
      prev_move_left = false;
      prev_move_fwd = true;
      break;
    
    case S1_IN_S2_OUT: 
      Serial.println("Sensor 2 is outside of black line");
      turnRight();
      prev_move_left = false;
      prev_move_fwd = false;
      break;
      
    case S1_OUT_S2_IN: 
      Serial.println("Sensor 2 is outside of black line"); 
      prev_move_left = true;
      prev_move_fwd = false;
      turnLeft();
      break;
    
    case S1_OUT_S2_OUT: 
      if(prev_move_fwd) {
        bothMotorsRun();
      }
      else if(prev_move_left) {
        turnLeft();
      } else {
        turnRight();
      } 
      
      Serial.println("Sensor 1 and 2 are outside of black line"); 
      break;
    
    default: break;
  }
}

O versiune a codului final (linia neagră):

Codul tău final poate arăta altfel. Nu e nevoie să copiezi acest cod. Aici ai doar un exemplu din care să te inspiri

#include "MeMCore.h"

MeLineFollower lineFinder(PORT_2); /* Line Finder module can only be connected to PORT_3, PORT_4, PORT_5, PORT_6 of base shield. */

MeDCMotor motor1(M1);
MeDCMotor motor2(M2);

float MOTOR1_TUNE = -1.0;
float MOTOR2_TUNE = 1.0;

uint8_t motorSpeed = 100;

bool prev_move_left = false;
bool prev_move_fwd = true;

void setup()
{
  // Optional: robotul porneste de la un buton
  pinMode(7, INPUT); //Define button pin as input
  while (analogRead(7) > 100) {
    delay(50); //Wait till button pressed to start.
  }
  
  // setez comunicarea prin portul serial
  Serial.begin(9600);
}

void activateLeftMotor() {
   motor1.run(MOTOR1_TUNE*motorSpeed); /* value: between -255 and 255. */
}

void activateRightMotor() {
   motor2.run(MOTOR2_TUNE*motorSpeed); /* value: between -255 and 255. */
}

void stopLeftMotor() {
   motor1.stop();
}

void stopRightMotor() {
   motor2.stop();
}

void turnLeft() {
  activateLeftMotor();
  stopRightMotor();
}

void turnRight() {
  activateRightMotor();
  stopLeftMotor();
}


void bothMotorsRun() {
  activateRightMotor();
  activateLeftMotor();
 }

void loop()
{
  int sensorState = lineFinder.readSensors();
  
  switch(sensorState)
  {
    case S1_IN_S2_IN: 
      Serial.println("Sensor 1 and 2 are inside of black line"); 
      bothMotorsRun();
      prev_move_left = false;
      prev_move_fwd = true;
      break;
    
    case S1_IN_S2_OUT: 
      Serial.println("Sensor 2 is outside of black line");
      turnRight();
      prev_move_left = false;
      prev_move_fwd = false;
      break;
      
    case S1_OUT_S2_IN: 
      Serial.println("Sensor 2 is outside of black line"); 
      prev_move_left = true;
      prev_move_fwd = false;
      turnLeft();
      break;
    
    case S1_OUT_S2_OUT: 
      if(prev_move_fwd) {
        bothMotorsRun();
      }
      else if(prev_move_left) {
        turnLeft();
      } else {
        turnRight();
      } 
      
      Serial.println("Sensor 1 and 2 are outside of black line"); 
      break;
    
    default: break;
  }
  
}