Here, this is my old testing code, you might be able to learn something from it and apply it to your project.
It is very primitive, and improper, but it does work. As and added bonus, this code also uses a Parallax ultrasonic ping sensor. (the 4 pin sensor)
/*
simple keyboard control test
*/
long duration, inches, cm;
const int ping = 7;//Sensor (trigger)
const int echo = 8;//Sensor (echo)
int distance;
char val; // variable to receive data from the serial port
byte M1L = 3;// PWM
byte M1R = 5;// PWM
byte M2L = 9;// PWM
byte M2R = 6;// PWM
void setup()
{
pinMode(M1L, OUTPUT);
pinMode(M1R, OUTPUT);
pinMode(M2L, OUTPUT);
pinMode(M2R, OUTPUT);
Serial.begin(9600); // start serial communication at 9600bps
}
void loop() {
if( Serial.available() ) // if data is available to read
{
val = Serial.read();
if( val == 'W' || val == 'w' )//forwards
{
//Serial.println("Moving Forwards");
digitalWrite(M1L, HIGH); digitalWrite(M2L, HIGH);
digitalWrite(M1R, LOW); digitalWrite(M2R, LOW);
}
else if( val == 'S' || val == 's' )//backwards
{
//Serial.println("Moving Backwards");
digitalWrite(M1L, LOW); digitalWrite(M2L, LOW);
digitalWrite(M1R, HIGH); digitalWrite(M2R, HIGH);
}
else if( val == 'A' || val == 'a' )//Left
{
//Serial.println("LEFT");
digitalWrite(M1L, LOW); digitalWrite(M2L, HIGH);
digitalWrite(M1R, HIGH); digitalWrite(M2R, LOW);
}
else if( val == 'D' || val == 'd' )
{
//Serial.println("RIGHT");
digitalWrite(M1L, HIGH); digitalWrite(M2L, LOW);
digitalWrite(M1R, LOW); digitalWrite(M2R, HIGH);
}
else { //full stop
digitalWrite(M1L, LOW); digitalWrite(M2L, LOW);
digitalWrite(M1R, LOW); digitalWrite(M2R, LOW);
}
if( val == 'R' || val == 'r' ){ //search mode
getping();}
}
}
void getping()
{
Serial.print("R is not pressed ");
while(val == 'r')
{
Serial.print("R is pressed ");
pinMode(ping, OUTPUT);
digitalWrite(ping, LOW);
delayMicroseconds(2);
digitalWrite(ping, HIGH);
delayMicroseconds(5);
digitalWrite(ping, LOW);
pinMode(echo, INPUT);
duration = pulseIn(echo, HIGH);
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.print(inches);
Serial.print("in, ");
Serial.print(cm);
Serial.print("cm ");
Serial.println();
delay(100);
//------Search for object with in range------
if(inches > 12)
{
Serial.println("Searching for object");
analogWrite(M1R, 100);
digitalWrite(M1L, LOW);
analogWrite(M2L, 100);
digitalWrite(M2R, LOW);
}
else if(inches >= 4 && inches <= 12)
{
analogWrite(M1R, 90);
digitalWrite(M1L, LOW);
analogWrite(M2L, 90);
digitalWrite(M2R, LOW);
Serial.println("Object Found!!!");//Move forward to object
if(inches <= 6){ distance = 450;}
else distance = 300;
delay(distance);
Serial.println("Going to object"); //is within 5 inches
while(inches != 3) //go to up object and stop when object
{
Serial.println(inches);
pinMode(ping, OUTPUT);
digitalWrite(ping, LOW);
delayMicroseconds(2);
digitalWrite(ping, HIGH);
delayMicroseconds(5);
digitalWrite(ping, LOW);
pinMode(echo, INPUT);
duration = pulseIn(echo, HIGH);
inches = microsecondsToInches(duration);
analogWrite(M1L, 100);
digitalWrite(M1R, LOW);
analogWrite(M2L, 100);
digitalWrite(M2R, LOW);
}
digitalWrite(M1L, LOW); //all motors stop
digitalWrite(M1R, LOW);
digitalWrite(M2L, LOW);
digitalWrite(M2R, LOW);
delay(500);
return;
}
if(val=='x')
{
Serial.print("Returning to prog ");
return;
}
else
{
Serial.print("X not pressed,");
}
}
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}