Here's my code. As you can see, I do print the values. I tried printing out the lowestVal and that worked fine, I just can't get the servo to move.
//This stuff just copies in servo library and sets a bunch of variables so I know which pins go to what
#include <Servo.h>
const byte trigPin1 = 2;
const byte echoPin1 = 3;
const byte trigPin2 = 4;
const byte echoPin2 = 5;
const byte trigPin3 = 6;
const byte echoPin3 = 7;
const byte trigPin4 = 8;
const byte echoPin4 = 9;
const byte trigPin5 = 10;
const byte echoPin5 = 11;
const byte servoPin = 12;
float duration;
unsigned int distance;
unsigned int servoPos = 0;
Servo myServo;
void setup() {
Serial.begin(9600);
//Sets the trig pins to outputs and echopins to inputs, so the sensors can work
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(trigPin4, OUTPUT);
pinMode(echoPin4, INPUT);
pinMode(trigPin5, OUTPUT);
pinMode(echoPin5, INPUT);
myServo.attach(servoPin);
myServo.write(servoPos);
}
void loop() {
//This is all stuff I just copied from an hc-sr04 guide, don't mess with it
digitalWrite(13, LOW);
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
int duration1 = pulseIn(echoPin1, HIGH);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
int duration2 = pulseIn(echoPin2, HIGH);
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
int duration3 = pulseIn(echoPin3, HIGH);
digitalWrite(trigPin4, LOW);
delayMicroseconds(2);
digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin4, LOW);
int duration4 = pulseIn(echoPin4, HIGH);
digitalWrite(trigPin5, LOW);
delayMicroseconds(2);
digitalWrite(trigPin5, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin5, LOW);
int duration5 = pulseIn(echoPin5, HIGH);
//More math from the guide
int distance1 = (duration1 * .0343) / 2;
int distance2 = (duration2 * .0343) / 2;
int distance3 = (duration3 * .0343) / 2;
int distance4 = (duration4 * .0343) / 2;
int distance5 = (duration5 * .0343) / 2;
if (distance5 < 200 || distance > 1000) {
distance5 = 1000;
} if (distance4 < 200 || distance4 > 1000) {
distance4 = 1000;
} if (distance3 < 100 || distance3 > 1000) {
distance3 = 1000;
} if (distance2 < 100 || distance2 > 1000) {
distance2 = 1000;
} if (distance1 < 100 || distance1 > 1000) {
distance1 = 1000;
}
//Takes the minimum of all 5 distances to see which is closest
int lowestVal = min(distance1, min(distance2, min(distance3, min(distance4, distance5))));
//This entire code thing only works if the lowest value is significant, not just if it's like 1000
if(lowestVal < 800){
//Depending on which sensor measures the shortest distance, this code moves the servo to that location
if(lowestVal == distance1){
myServo.write(servoPos);
}
if(lowestVal == distance2){
myServo.write(servoPos+36);
}
if(lowestVal == distance3){
myServo.write(servoPos+72);
}
if(lowestVal == distance4){
myServo.write(servoPos+108);
}
if(lowestVal == distance5){
myServo.write(servoPos+144);
}
//Gives enough delay for servo to move to the right location
delay(1000);
//Prints out values of each distance measurement
Serial.println("Distances: ");
Serial.println(distance1);
Serial.println(distance2);
Serial.println(distance3);
Serial.println(distance4);
Serial.println(distance5);
Serial.println(lowestVal);
//If the servo has rotated to its maximum in the cycle, it switches back to 0. Otherwise, it adds 4 degrees
if(servoPos == 32){
servoPos = 0;
delay(400);
}
else{
servoPos = servoPos + 4;
delay(10);
}
}
}