// pins and misc constants const int stepPinM2 = 11; const int dirPinM2 = 10; const int stepPinM1 = 9; const int dirPinM1 = 8; const int buttonPin = 7; const int yellowPin = 2; const int greenPin = 1; const int iPin = 4; const int iPinA = 3; const int jPin = 6; const int jPinA = 5; const float a = 5.69; const float b = 65; // misc variables int goodGo = 0; int button = 0; int start = 0; float y; float i; float j; float h; float iRaw; float iPos; float jRaw; float jPos; float deg; float degCalc; float rad; float pos = 90; float goal = 0; void setup() { // set pinmodes pinMode(stepPinM1,OUTPUT); pinMode(dirPinM1,OUTPUT); pinMode(stepPinM2,OUTPUT); pinMode(dirPinM2,OUTPUT); pinMode(buttonPin,INPUT_PULLUP); pinMode(yellowPin,OUTPUT); pinMode(greenPin,OUTPUT); pinMode(iPin,INPUT); pinMode(iPinA,OUTPUT); pinMode(jPin,INPUT); pinMode(jPinA,OUTPUT); } void loop() { button = digitalRead(buttonPin); //activate the dist. sensors digitalWrite(iPinA,LOW); delayMicroseconds(2); digitalWrite(iPinA,HIGH); delayMicroseconds(10); digitalWrite(iPinA,LOW); digitalWrite(jPinA,LOW); delayMicroseconds(2); digitalWrite(jPinA,HIGH); delayMicroseconds(10); digitalWrite(jPinA,LOW); iRaw = pulseIn(iPin,HIGH); jRaw = pulseIn(jPin,HIGH); // blink yellow lamp in for setup. takes 5 seconds, put your hand in possision. if (button == HIGH && goodGo == 0){ for (int x = 0; x < 5, x++;) { digitalWrite(yellowPin,HIGH); delayMicroseconds(500000); digitalWrite(yellowPin,LOW); delayMicroseconds(500000); } // possision of your hand is saved, the green light turns on digitalWrite(greenPin,HIGH); goodGo = 1; iPos = pulseIn(iPin,HIGH); jPos = pulseIn(jPin,HIGH); } // reset setup if button is activated if (button == LOW) { goodGo = 0; } // calculate the vector (i,j) if (iPos < iRaw - 100 || iPos > iRaw +100 || jPos < jRaw - 100 || jPos > jRaw +100){ if (iPos < iRaw) {i = iRaw-iPos;} if (iPos > iRaw) {i = iPos-iRaw;} if (jPos < jRaw) {j = jRaw-jPos;} if (jPos > jRaw) {j = jPos-jRaw;} } else { i = 0; j = 0;} // calculate the degree of where your hand moved h = sqrt((i*i)+(j*j)); rad = asin((h/sin(90))/i); degCalc = (rad*180)/3.14; // calculate the goal the wheel needs to turn to if (i > 0 && j > 0 || i < 0 && j < 0){ goal = degCalc + 45;} if ((i > 0 && j < 0 || i > 0 && j < 0) && degCalc > 45){ goal = 45- degCalc;} if ((i > 0 && j < 0 || i > 0 && j < 0) && degCalc >= 45){ goal = 180- degCalc;} // set the deriction of the wheel if (pos > goal) {digitalWrite(dirPinM1,LOW);deg = pos-goal;} if (pos <= goal) {digitalWrite(dirPinM1,HIGH);deg = goal-pos;} // set or deactivate "start" if button is on or off if (button == HIGH){ start = 1; } else { start = 0; } // uses degrees to rotate the wheel y = a*deg+b; if (start == 1 && y > b){ for(int x = 0; x < y; x++){ digitalWrite(stepPinM1,HIGH); delayMicroseconds(50000); digitalWrite(stepPinM1,LOW); delayMicroseconds(50000); } } // set diretion for the wheel to go if ( i > 0 || j > 0){ digitalWrite(dirPinM2,LOW);} if ( i < 0 || j < 0){ digitalWrite(dirPinM2,HIGH);} // starts the wheel if (start == 1 && (i != 0 || j !=0)){ digitalWrite(stepPinM2,HIGH); delayMicroseconds(50000); digitalWrite(stepPinM2,LOW); delayMicroseconds(50000); } }