player456/challenge3.cpp at main · AveryLor/player456 · 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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
// motor pins
int leftPin1 = 2;
int leftPin2 = 3;
int rightPin1 = 4;
int rightPin2 = 5;

// color pins
int color0 = 6;
int color1 = 7;
int color2 = 8;
int color3 = 9;
int EN = 13;
int colorOut = 12;

int redMin = 67;
int redMax = 900;
int greenMin = 102;
int greenMax = 1300;
int blueMin = 95;
int blueMax = 1000;

int redPW, greenPW, bluePW = 0;
int red, blue, green;
long duration = 0, distance = 0;

int seq = 0;
int time = 0;
int time180 = 6;
int time90 = 3;

typedef enum {
  FORWARD,
  LEFT,
  RIGHT,
  STOP
} movement;

movement state = FORWARD;

void setup() {
  // put your setup code here, to run once:
  // motor control
  pinMode(leftPin1, OUTPUT);
  pinMode(leftPin2, OUTPUT);
  pinMode(rightPin1, OUTPUT);
  pinMode(rightPin2, OUTPUT);

  pinMode(A4, OUTPUT);
  pinMode(A5, INPUT);

  pinMode(color0, OUTPUT);
  pinMode(color1, OUTPUT);
  pinMode(color2, OUTPUT);
  pinMode(color3, OUTPUT);
  pinMode(colorOut, INPUT);

  pinMode(A1, OUTPUT);

  pinMode(EN, OUTPUT);
  digitalWrite(EN, LOW);

  digitalWrite(color0, HIGH);
  digitalWrite(color1, LOW);
  digitalWrite(A1, LOW);

  Serial.begin(9600);

  // speed control
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:

  //Controlling speed (0 = off and 255 = max speed):
  analogWrite(10, 110);  //ENA pin
  analogWrite(11, 110);  //ENB pin

  digitalWrite(A4, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(A4, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(A4, LOW);
  duration = pulseIn(A5, HIGH);
  distance = (duration/2) / 29.1;
  Serial.println(distance);


  if (distance > 15 && time == 0) {
    state = FORWARD;
  } else if (time == 0) {
    state = RIGHT;
    time = 3;
  }

  if (time > 0) {
    time--;
  }

  switch (seq) {
    case 0:
      if (red > 170) {
        digitalWrite(A1, HIGH);
        delay(500);
        digitalWrite(A1, LOW);
        seq = 1;
      }
      break;
    case 1:
      if (green > 150 && blue < 150) {
        digitalWrite(A1, HIGH);
        delay(500);
        digitalWrite(A1, LOW);
        seq = 2;
      }
      break;
    case 2:
      if (green > 150 && blue > 150) {
        digitalWrite(A1, HIGH);
        delay(500);
        digitalWrite(A1, LOW);
        seq = 3;
      }
      break;
    case 3:
      if (green > 150 && blue < 150) {
        digitalWrite(A1, HIGH);
        delay(500);
        digitalWrite(A1, LOW);
        seq = 4;
      }
      break;
    case 4:
      if (green > 150 && blue > 150) {
        digitalWrite(A1, HIGH);
        delay(500);
        digitalWrite(A1, LOW);
        seq = 5;
      }
      break;
    case 5:
      stop = 1;
      break;
  }


  // Color Sensor
  redPW = getRed();
  red = map(redPW, redMin, redMax, 255, 0);
  delay(200);

  greenPW = getGreen();
  green = map(greenPW, greenMin, greenMax, 255, 0);
  delay(200);

  bluePW = getBlue();
  blue = map(bluePW, blueMin, blueMax, 255, 0);
  delay(200);


}