LineFollowingRobotFirmware/robot_logic.c at master · john-curry/LineFollowingRobotFirmware · 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
#include "robot_logic.h"
bool forward_Logic(Input * in, State * c) {
  if (on_Line(in) || off_Right(in) || off_Left(in)) {
    if (off_Left(in) && !off_Right(in)) {
      set_State("correct_left", c);
      return false;
    }
    if (off_Right(in) && !off_Left(in)) {
      set_State("correct_right", c);
      return false;
    }
    if (off_Right(in) && off_Left(in)) {
      set_State("move_forward", c);
      return false;
    }
    if (off_Right(in)) {
      set_State("correct_right", c);
      return false;
    }
    if (off_Left(in)) {
      set_State("correct_left", c);
      return false;
    }
  }
  else {
    if (line_Left(in) && !line_Right(in)) {
      set_State("turn_left", c);
      return false;
    }
    if (line_Right(in) && !line_Left(in)) {
      set_State("turn_right", c);
      return false;
    }
    if (line_Right(in) && line_Left(in)) {
      set_State("move_forward", c);
      return false;
    }
    if (line_Right(in)) {
      set_State("turn_right", c);
      return false;
    }
    if (line_Left(in)) {
      set_State("turn_left", c);
      return false;
    }
  }
  set_State("move_forward", c);
  return false;
}

void left_Logic(Input * in, State * c) {
  if ((is_State("reverse_right", c) || is_State("reverse_left", c)) || is_State("turn_around", c)) {
    return;
  }
  set_State("reverse_left", c);
}

void right_Logic(Input * in, State * c) {
  if ((is_State("reverse_right", c) || is_State("reverse_left", c)) || is_State("turn_around", c)) {
    return;
  }
  set_State("reverse_right", c);
}

void back_Logic(Input * in, State * c) {
  if ((is_State("reverse_right", c) || is_State("reverse_left", c)) || is_State("turn_around", c)) return;
  set_State("turn_around", c);
}