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> 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 & Y values lAngle = map(X, X_MIN_DISTANCE, X_MAX_DISTANCE, -MAX_ANGLE, MAX_ANGLE); // Using X & 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(" --> "); Serial.println(lAngle); Serial.print("R: "); Serial.print(rightDistance); Serial.print(" --> "); 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 > MAX_ANGLE) { // Left lAngle = MAX_ANGLE; } if (lAngle < -MAX_ANGLE) { lAngle = -MAX_ANGLE; } if (rAngle > MAX_ANGLE) { // Right rAngle = MAX_ANGLE; } if (rAngle < -MAX_ANGLE) { rAngle = -MAX_ANGLE; } } // Smooth/incremented servo movement void moveServo(Servo &s, int angle) { // Pass servo by reference int previousAngle = s.read(); if (angle > previousAngle) { for (int i = previousAngle; i <= angle; i++) { s.write(i); delay(10); } } if (angle < previousAngle) { for (int i = previousAngle; i >= angle; i--) { s.write(i); delay(10); } } } // Function to calculate the realtive X & Y distances of the sensed object // :: an approach using Heron's formula void getCoordinates() { // Get Heron variables float a = float(leftDistance); //d2 (vertex -> sensor B) float b = float(rightDistance); //d1 (vertex -> 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 > d) || //d1 out-of-range (a > 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); } } }
Leave a Reply
You must be logged in to post a comment.