Maze Assembly

During the past week, we laser cut and assembled the main structure. We also managed to tilt the board on two axes as intended using two sonar sensors.

Experimental Hands Free Control

We also tested a new handsfree control scheme where, using two sonar sensors, a user can move an object on a 2D plane in order to control the maze tilt:

2-sensor setup which uses Heron’s formula to calculate X & Y position
*the shaded area is the detection field

Using this scheme, the formula generates really accurate and consistent Y-values, with a jumpy and inconsistent range of X-values. The setup can benefit from a smoothing-filter to reduce the jumpiness of the Servos.

Winner Feedback

Finally, we added a Phototransistor & an LED to our project that would rapidly blink an LED once a phototransistor was triggered by a passing marble. This threshold should be determined after some tests, so that the LED does not get triggered by a change in lighting.

See the PHOTORESISTOR_PIN and LED_PIN_WIN below.

Code Artifacts

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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
#include <Servo.h&gt;
bool DEBUG = false; // Print output
 
 
// CONSTANTS
const int X_SERVO_PIN = 6;
const int Y_SERVO_PIN = 7;
 
const int L_ECHO_PIN = 13;
const int L_TRIGGER_PIN = 12;
const int R_ECHO_PIN = 11;
const int R_TRIGGER_PIN = 10;
 
const int PHOTORESISTOR_PIN = A0;
const int LED_PIN_WIN = 9; // A flashing LED for finishing the maze
 
// Change the following as needed
const float BASELINE = 50.0; // Triangle Baseline
const int MAX_ANGLE = 20;  // Max angle that the servo will rotate in either direction
const int START_ANGLE = 15; // Starting angle for a servo
 
//const int MAX_DISTANCE = 100; // cm
const int X_MAX_DISTANCE = 30;
const int X_MIN_DISTANCE = 10;
const int Y_MAX_DISTANCE= 70;
const int Y_MIN_DISTANCE= 30;
 
// GLOBALS
Servo xServo;
Servo yServo;
float leftDistance; // Hand distance in cm
float rightDistance;
int X = 20; // X coordinate
int Y = 50; // Y coord
int lAngle; // Corresponding servo angle
int rAngle;
float lightLevel = 1023; // kOhm
 
// FUNCTIONS
void setup()
{
  Serial.begin(9600);
  xServo.attach(X_SERVO_PIN);
  yServo.attach(Y_SERVO_PIN);
  pinMode(X_SERVO_PIN, OUTPUT);
  pinMode(Y_SERVO_PIN, OUTPUT);
  pinMode(LED_PIN_WIN, OUTPUT);
  pinMode(PHOTORESISTOR_PIN, INPUT);
  xServo.write(START_ANGLE);
  yServo.write(START_ANGLE);
}
 
void loop()
{
  // Get hardware input
  leftDistance = 0.01723 * handDistance(L_TRIGGER_PIN, L_ECHO_PIN);
  rightDistance = 0.01723 * handDistance(R_TRIGGER_PIN, R_ECHO_PIN);
 
  getCoordinates(); // set the X &amp; Y values
 
  lAngle = map(X, X_MIN_DISTANCE, X_MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE); // Using X &amp; Y coordinates
  rAngle = map(Y, Y_MIN_DISTANCE, Y_MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE);
//  lAngle = map(leftDistance, 0, MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE); // Using sensor distances directly
//  rAngle = map(rightDistance, 0, MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE);
 
  // Sanitize input
  sanitizeAngles();
 
  // Move servos
  moveServo(xServo, START_ANGLE + lAngle); // L
  moveServo(yServo, START_ANGLE + rAngle); // R
 
  // See whether the phototransistor was activated
  pollPhotoresistor();
 
  Serial.println("");
  Serial.print("LL: ");
  Serial.println(lightLevel);
 
  if (DEBUG) {
    Serial.print("L: ");
    Serial.print(leftDistance);
    Serial.print(" --&gt; ");
    Serial.println(lAngle);
 
    Serial.print("R: ");
    Serial.print(rightDistance);
    Serial.print(" --&gt; ");
    Serial.println(rAngle);
  }
 
  delay(100);
}
 
long handDistance(int TRIGGER_PIN, int ECHO_PIN)
{
  pinMode(TRIGGER_PIN, OUTPUT);
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  pinMode(ECHO_PIN, INPUT);
  return pulseIn(ECHO_PIN, HIGH);
}
 
// Keeps the calculated angles within their bounds
void sanitizeAngles() {
  if (lAngle &gt; MAX_ANGLE) { // Left
    lAngle = MAX_ANGLE;
  }
  if (lAngle < -MAX_ANGLE) {
    lAngle = -MAX_ANGLE;
  }
  if (rAngle &gt; MAX_ANGLE) { // Right
    rAngle = MAX_ANGLE;
  }
  if (rAngle < -MAX_ANGLE) {
    rAngle = -MAX_ANGLE;
  }
}
 
// Smooth/incremented servo movement
void moveServo(Servo &amp;s, int angle) { // Pass servo by reference
  int previousAngle = s.read();
 
  if (angle &gt; previousAngle)
  {
    for (int i = previousAngle; i <= angle; i++)
    {
      s.write(i);
      delay(10);
    }
  }
 
  if (angle < previousAngle)
  {
    for (int i = previousAngle; i &gt;= angle; i--)
    {
      s.write(i);
      delay(10);
    }
  }
}
 
// Function to calculate the realtive X &amp; Y distances of the sensed object
// :: an approach using Heron's formula
void getCoordinates() {
  // Get Heron variables
  float a = float(leftDistance);    //d2 (vertex -&gt; sensor B)
  float b = float(rightDistance);                       //d1 (vertex -&gt; sensor A)
  float c = BASELINE;                               //baseline
  //float d = c*1.414;                              //display diagonal (square)
  float d = sqrt(150 * 150 + 100 * 100);            //diagonal (display + offset)
  float s = (a + b + c) / 2;                        //semi-perimeter
 
  if
  (
    (a < 0) ||          //d1 must be less than d2
    (b &gt; d) ||          //d1 out-of-range
    (a &gt; d) ||          //d2 out-of-range
    ((s - a) < 0) ||    //these values must be positive
    ((s - b) < 0) ||
    ((s - c) < 0)
  )
  {
    if(DEBUG) {
      Serial.println("BAD VAL");
    }
    return; // Dont change the coordinates
  }
 
  float area = sqrt(s * (s - a) * (s - b) * (s - c));
  Y = area * 2 / c;
  X = sqrt(b * b - Y * Y);
 
  if (DEBUG) {
//    Serial.print("    d1: ");
//    Serial.println(b);
//    Serial.print("    d2: ");
//    Serial.println(a);
//    Serial.print("  base: ");
//    Serial.println(c);
//    Serial.print("     s: ");
//    Serial.println(s);
//    Serial.print("  area: ");
//    Serial.println(area);
    Serial.print("     X: ");
    Serial.println(X);
    Serial.print("     Y: ");
    Serial.println(Y);
    Serial.println("");
  }
}
 
// Triggers the LED celebration when the phototransistor was triggered
void pollPhotoresistor() {
  lightLevel = (float)analogRead(PHOTORESISTOR_PIN); // Input range: 1023 - 319
 
  // Blink LED if the resistor was triggered
  if (lightLevel  < 1000)
    for(int i=0; i<3; i++) {
      digitalWrite(LED_PIN_WIN, HIGH);
      delay(500);   
      digitalWrite(LED_PIN_WIN, LOW);
      delay(500); 
    }
  }
}