LineFollowingRobot/LineFollower.ino at master · JACTheCreator/LineFollowingRobot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include <Servo.h>

Servo rightServo;
Servo leftServo;

/**
 * The setup() function is called when a sketch starts.
 * Use it to initialize variables, pin modes, start using libraries, etc.
 */
void setup()
{
  pinMode(11, INPUT);

  /**
   * Configures the specified pins on the sensors to behave an input.
   */
  pinMode(2, INPUT); //s0
  pinMode(3, INPUT); //s1
  pinMode(4, INPUT); //s2
  pinMode(5, INPUT); //s3
  pinMode(6, INPUT); //s4
  pinMode(7, INPUT); //s5
  pinMode(8, INPUT); //s6
  pinMode(9, INPUT); //s7
}


void loop()
{
  followLine(digitalRead(11));
}

/**
 * If the value is:
 * 0/false/LOW -> Follows a white line on a black surface
 * 1/true/HIGH -> Follows a black line on a white surface
 */
void followLine(uint8_t lineColour)
{
  if(digitalRead(5) == lineColour && digitalRead(6) == lineColour)
  {
    forward();
  }
  else if(digitalRead(2) == lineColour || digitalRead(3) == lineColour || digitalRead(4)== lineColour || digitalRead(5) == lineColour)
  {
    right();
  }
  else if(digitalRead(6) == lineColour || digitalRead(7) == lineColour || digitalRead(8) == lineColour || digitalRead(9) == lineColour)
  {
    left();
  }
  else
  {
    stop();
  }
}

/**
 *
 */
void forward()
{
  checkAttachedStatus();
  leftServo.write(-180);
  rightServo.write(180);
}

/*
 *
 */
void reverse()
{
  checkAttachedStatus();
  leftServo.write(180);
  rightServo.write(-180);
}

/*
 *
 */
void left()
{
  checkAttachedStatus();
  leftServo.write(-180);
  rightServo.write(-180);
}

/*
 *
 */
void right()
{
  checkAttachedStatus();
  leftServo.write(180);
  rightServo.write(180);
}

/*
 *
 */
void stop()
{
 rightServo.detach();
 leftServo.detach();
}

/*
 *
 */
void checkAttachedStatus()
{
  if(!leftServo.attached() && !rightServo.attached())
  {
    rightServo.attach(12);
    leftServo.attach(13);
  }
}