//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int pwm_a = 3;
int valgus1 = 4;
int valgus2 = 5;
int V52 = 6;
int GND2 = 7;
int pin = 8;
int GND = 9;
int V5 = 10;
int pwm_b = 11;
int dir_a = 12;
int dir_b = 13;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
unsigned long time;
unsigned long sizeofpulse;
float distance;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
Serial.begin(9600);
pinMode(V5, OUTPUT);
pinMode(GND, OUTPUT);
pinMode(V52, OUTPUT);
pinMode(GND2, OUTPUT);
pinMode(pin, OUTPUT);
pinMode(pwm_a, OUTPUT);
pinMode(pwm_b, OUTPUT);
pinMode(dir_a, OUTPUT);
pinMode(dir_b, OUTPUT);
digitalWrite(GND, LOW);
digitalWrite(GND2, LOW);
digitalWrite(V5, HIGH);
digitalWrite(V52, HIGH);
digitalWrite(pin, HIGH);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
pinMode(pin, OUTPUT); //return digital pin to OUTPUT mode after reading
digitalWrite(pin, LOW);
delayMicroseconds(25);
digitalWrite(pin, HIGH); //Trig pin pulsed LOW for 25usec
time = micros(); //record timer
pinMode(pin, INPUT); //change pin to INPUT to read the echo pulse
sizeofpulse = pulseIn(pin, LOW, 18000); //should be approx 150usec, timeout at 18msec
time = micros() - time - sizeofpulse; // amount of time elapsed since we sent the trigger pulse and detect the echo pulse, then subtract the size of the echo pulse
distance = (time*340.29/2/10000)-3; // convert to distance in centimeters
int Valgus_Value = readQD();
Serial.println(Valgus_Value);
int Valgus_Value2 = readQD2();
Serial.println(Valgus_Value2);
Serial.print(" Kaugus: ");
Serial.print(distance);
Serial.println(" cm");
delay(10);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if (distance >= 25) //Anduri threshold
{
digitalWrite(dir_a, LOW);
digitalWrite(dir_b, LOW);
delay(10);
analogWrite(pwm_a, 100);
analogWrite(pwm_b, 100);
delay(10);
}
else if (distance <= 25) //Anduri threshold
{
digitalWrite(dir_a, LOW);
digitalWrite(dir_b, HIGH);
delay(10);
analogWrite(pwm_a, 255);
analogWrite(pwm_b, 255);
delay(10);
}
if (Valgus_Value <= 255) //Anduri threshold
{
digitalWrite(dir_a, HIGH);
digitalWrite(dir_b, LOW);
delay(10);
analogWrite(pwm_a, 255);
analogWrite(pwm_b, 100);
delay(10);
}
if (Valgus_Value2 <= 255) //Anduri threshold
{
digitalWrite(dir_a, HIGH);
digitalWrite(dir_b, LOW);
delay(10);
analogWrite(pwm_a, 255);
analogWrite(pwm_b, 100);
delay(10);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
int readQD(){
pinMode( Valgus1, OUTPUT );
digitalWrite( Valgus1, HIGH );
delayMicroseconds(10);
pinMode( Valgus1, INPUT );
long time2 = micros();
while (digitalRead(Valgus1) == HIGH && micros() - time2 < 3000);
int diff = micros() - time2;
return diff;
}
int readQD2(){
//Returns value from the QRE1113
//Lower numbers mean more refleacive
//More than 3000 means nothing was reflected.
pinMode( Valgus2, OUTPUT );
digitalWrite( Valgus2, HIGH );
delayMicroseconds(10);
pinMode( Valgus2, INPUT );
long time3 = micros();
//time how long the input is HIGH, but quit after 3ms as nothing happens after that
while (digitalRead(Valgus2) == HIGH && micros() - time3 < 3000);
int diff2 = micros() - time3;
return diff2;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Ouh yeha forgot to tell im using motor shield and i give 5 Volts from batteries to the motor shield. aka the two pins next to A and B channel.
So but the power got me to relaise if the below arduino uno board even gets pwoer when i power the motor shield.
And this makes sense cause when i plug the arduino in it works when unpluged the board stops working.
Edit:
Holy cow the code has errors. " Feeling ashemed of myself "
Im using outputs for Sensors. almost fried the sensors while i was giving the arduino uno board 9V. 
Glad that i ralaised it before. xD kk. code needs to be more fixed and i know the code is half copy paste from other sources. SO would need to link them when i link it here.
im using digital colour sensors QRE.
ANd by the power it seems that Motor Shield and arduino uno need diffrent power supplies or if i give motor shield power the uno gets it. That i dont know and i dont know if or how INPUT and OUTPUT work properly cause the refrence page says something but without examples its hard to understand it. And the only code part that is done by myself is the IF statements.
And from the connections its simple. I got Motor shield ontop of the Arduino UNO + and - going to the Motor shield VIn and gnd plugs, Then from the code itself you can see where the sensors are located and yeah the code aint pretty. xD But will write here what sensor is where.
QRE ( Light Sensor ) 2 of them :
Power supply. is at 6 & 10. using OUTPUT HIGH
GND is 7 and one is at the Boards self GND
Read sensor: 4 and 5 pins.
Ultrasonic:
Power: 0.o kk missing. Need to chekc the machine itself for this.
GND: 0.o Kk missing. Need to check the machine itself for this.
Read: Pin 8
Motors are connected to Channel A & B on the motor shield
using PWM to run them.
