Hands-free Balance Maze :: V2 Prototype

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:

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

#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);  
    }
  }
}