Team: Yu Jiang & Max Kornyev

Prototype Video

Progress Made

  • Remote support
    • There is now a LISTEN_INCOMING_MESSAGES mode
  • Two layers of filtering operations
    • Smoothing the calculation outputs
      • Heron’s Formula is used to calculate an X & Y position for the detected item
    • Applying a highpass filter to (or smoothing) the servo positions
  • Visual feedback with flashing LED support

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
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
#include <Servo.h&gt;
 
// MODES
bool DEBUG_COORDINATES = false; // Print output (coords computed first)
bool DEBUG_ANGLES = true; // (angles computed from coords)
bool LISTEN_INCOMING_MESSAGES = true; // Accept remote input?
 
// 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
 
// TUNE the following as needed
// Ctrl + F for "TUNE" to find all other tunable params
const float BASELINE = 70.0; // Triangle Baseline
const int MAX_ANGLE = 20;  // Max that the servo will rotate in either direction from STARTING_ANGLE
const int START_ANGLE = 15; // Starting angle for a servo
const int SERVO_INCREMENT_DELAY = 10; // Delay between 1-degree servo increments
 
int X = 35; // Starting X coordinate
int Y = 60; // Starting Y coord
 
const int X_MAX_DISTANCE = 44;
const int X_MIN_DISTANCE = 25;
const int Y_MAX_DISTANCE= 80;
const int Y_MIN_DISTANCE= 40;
 
// GLOBALS
Servo xServo;  // Servos
Servo yServo;
float leftDistance; // Hand distance in cm
float rightDistance;
int lAngle; // Corresponding servo angle
int rAngle;
float lightLevel = 1023; // kOhm
 
void setup()
{
  Serial.begin(115200);
  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()
{
  if(LISTEN_INCOMING_MESSAGES) { // If listening for messages
    while(Serial.available()) {
      lAngle = Serial.parseInt(); Serial.find(',');
      rAngle = Serial.parseInt(); Serial.find('\n');
    }
  } else {
    // Get raw distances
    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 coordinates
   
    int rawLAngle = map(X, X_MIN_DISTANCE, X_MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE); // Using X &amp; Y coordinates
    int rawRAngle = map(Y, Y_MIN_DISTANCE, Y_MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE);
   
    // Use 2nd smoothing filter
    lAngle = getSmoothedValue(lAngle, rawLAngle, 1);   // TUNE: 1 langle-multiplier (1 = NO DELAY)
    rAngle = getSmoothedValue(rAngle, rawRAngle, 0.8); // TUNE: 0.8 rangle-multiplier
     
    // OR use a highpass filter (with smaller movement area, but more control)
  //  lAngle = highpass(rawLAngle);
  //  rAngle = highpass(rawRAngle);
   
    sanitizeAngles(); // Sanitize angle input
 
    // Publish the calculated angles
    PUBLISH(lAngle, rAngle);
  }
 
  // Move servos
  moveServo(xServo, START_ANGLE + lAngle);
  moveServo(yServo, START_ANGLE + rAngle);
  
//  pollPhotoresistor(); // See whether the phototransistor was activated &amp; play win sequence
 
  if (DEBUG_ANGLES) {
    Serial.print("L: ");
    Serial.println(lAngle);
    Serial.print("R: ");
    Serial.println(rAngle);
//    Serial.print("LL: ");
//    Serial.println(lightLevel);
  }
 
  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);
}
 
// Simple smoothing function for a given value
int getSmoothedValue(float value, float input, float coeff) {
  float difference = input - value;
  return (value + (int)(coeff * difference));
}
 
// Simple highpass filter
// - for supressing occasionall (low-freqency) erroneous vals
float highpass(float input) {
  float output = input;
  {
    static float z1, z2;
    float x = output - -1.04859958*z1 - 0.29614036*z2;
    output = 0.43284664*x + -0.86569329*z1 + 0.43284664*z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2;
    float x = output - -1.32091343*z1 - 0.63273879*z2;
    output = 1.00000000*x + -2.00000000*z1 + 1.00000000*z2;
    z2 = z1;
    z1 = x;
  }
  return output;
}
 
// 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 sevo by reference
  int previousAngle = s.read();
 
  if (angle &gt; previousAngle)
  {
    for (int i = previousAngle; i <= angle; i++)
    {
      s.write(i);
      delay(SERVO_INCREMENT_DELAY);
    }
  }
 
  if (angle < previousAngle)
  {
    for (int i = previousAngle; i &gt;= angle; i--)
    {
      s.write(i);
      delay(SERVO_INCREMENT_DELAY);
    }
  }
}
 
// Function to calculate the realtive X &amp; Y distances of the sensed object
// Using Heron's formula &amp; some Pythagoras
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) ||         
    (b &gt; d) ||         
    (a &gt; d) ||         
    ((s - a) < 0) ||   
    ((s - b) < 0) ||
    ((s - c) < 0)
  )
  {
    Serial.println("BAD VAL");
    return; // Dont change the coordinates
  }
 
  float area = sqrt(s * (s - a) * (s - b) * (s - c));
  Y = getSmoothedValue(Y, (area * 2 / c), 0.9);      // TUNE: 0.9 y-multiplier
  X = getSmoothedValue(X, sqrt(b * b - Y * Y), 0.65); // TUNE: 0.65 x-multiplier
 
  if (DEBUG_COORDINATES) {
    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) {                          // TUNE: 1000 threshold
    for(int i=0; i<3; i++) {
      digitalWrite(LED_PIN_WIN, HIGH); delay(500);   
      digitalWrite(LED_PIN_WIN, LOW); delay(10); 
    }
  }
}
 
// Publishes message to MQTT via Serial
void PUBLISH(int a, int b) {
  if(DEBUG_COORDINATES || DEBUG_ANGLES) {
    Serial.println("ERROR: cannot print DEBUG and PUBLISH @ the same time");
    return;
  }
  Serial.print(a); Serial.print(","); Serial.print(b); Serial.print("\n");
}