NOTE: READ THE BELOW POST FIRST I MESSED UP AND WENT OVER ON CHARACTERS
Here'e the code (ignore any commented-out code and subroutines that are not called upon):
int servoturretPin = 3;
int pulses = 0;
int sensVal[10];
int prevSensVal[10];
int ultraSoundSignal = 7; // Ultrasound signal pin
int val = 0;
int ultrasoundValue = 0;
int timecount = 0; // Echo counter
int ledPin = 8; // LED connected to digital pin 13
boolean increment;
int i;
int prevcnt;
int newcnt;
int done;
int a;
int x;
int prevtime;
int delaytime;
void setup() {
Serial.begin(9600);
pinMode(servoturretPin, OUTPUT);
pinMode(ledPin, OUTPUT);
}
void loop()
{
Scan();
}
void Scan() {
if(done == 2) { // This if for debugging; it scans as much as it needs to to get results, then shows you them
printVals();
}
if(pulses >= 2100) // Starting here is the code for handling the back-and-forth of the servo
{ pulses = 2100;
increment = true;
while(pulses >= 1100)
{
prevtime = millis(); // Here I TRIED to fix the timing problem; it doesn't work, though
readPing(); // read the ultrasonic rangefinder
pulses = pulses - 50; // Speed
digitalWrite(servoturretPin, HIGH);
delayMicroseconds(pulses);
digitalWrite(servoturretPin, LOW);
delay(delaytime); // Speed
}
if(millis() > 10000) {
done = done + 1;
}
a = 0;
}
else if(pulses <= 1100)
{ pulses = 1100;
increment = false;
while(pulses <= 2100)
{
prevtime = millis();
readPing(); // read it again in reverse direction
pulses = pulses + 50; // Speed
digitalWrite(servoturretPin, HIGH);
delayMicroseconds(pulses);
digitalWrite(servoturretPin, LOW);
delay(delaytime); // Speed
}
if(millis() > 10000) {
done = done + 1;
}
a = 0;
}
} // (end of code for handling servo)
void readPing() {
timecount = 0;
val = 0;
pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
/* Send low-high-low pulse to activate the trigger pulse of the sensor
* -------------------------------------------------------------------
*/
digitalWrite(ultraSoundSignal, LOW); // Send low pulse
delayMicroseconds(2); // Wait for 2 microseconds
digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
delayMicroseconds(5); // Wait for 5 microseconds
digitalWrite(ultraSoundSignal, LOW); // Holdoff
/* Listening for echo pulse
* -------------------------------------------------------------------
*/
pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
val = digitalRead(ultraSoundSignal); // Append signal value to val
while(val == LOW) { // Loop until pin reads a high value
val = digitalRead(ultraSoundSignal);
}
while(val == HIGH) { // Loop until pin reads a high value
val = digitalRead(ultraSoundSignal);
timecount = timecount +1; // Count echo pulse time
}
/* Writing out values to the serial port
* -------------------------------------------------------------------
*/
ultrasoundValue = timecount; // Append echo pulse time to ultrasoundValue
Serial.println(ultrasoundValue);
if(pulses == 1100 || pulses == 1200 || pulses == 1300 || pulses == 1400 || pulses == 1500 || pulses == 1600 || pulses == 1700 || pulses == 1800 || pulses == 1900 || pulses == 2000 || pulses == 2100 && millis() > 10000 ) {
if(increment == true) { // The function of the above 'if' is to ONLY store new values every time pulses is 100 more
prevSensVal[a] = ultrasoundValue; // ...(store a total of 10 values in each direction)
a++;
} // The above 'if' is for storing the first set of values
if(increment == false) { // and <---- this one if for storing the next set (after reversing direction)
sensVal[a] = ultrasoundValue;
a++;
}
}
for(x = 0; x < 9; x++) { // For debugging
if(prevSensVal[x] > 0) {
prevcnt = prevcnt + 1;
}
if(sensVal[x] > 0) {
newcnt = newcnt + 1;
}
}
delaytime = 150 - (millis() - prevtime);
}
void printVals() {
Serial.println("Counters: ");
Serial.println(prevcnt);
Serial.println(newcnt);
Serial.println("Previous");
for(i = 0; i < 10; i++) {
Serial.println(prevSensVal[i]);
}
Serial.println("Newest");
for(i = 0; i < 10; i++) {
Serial.println(sensVal[i]);
}
end();
}
void end() { // ignore
delay(500);
end();
}
