/**
drawingByDistance
Author: Carlos Ortega
This code is used to move two motors at the same time using the distance measured by the PING))) sensor
We also have three butons that are used for calibration.
*/
#include <Servo.h>
//the pins that correspond to the buttons
const int leftButton = 2;
const int rightButton = 3;
const int stopButton = 4;
//the pin that corresponds to the PING)))
const int pingPin =7;
const int maxDist = 50; //in inches
const int minDist = 3; //also in inches
//servo vars
Servo myservo;
int leftOrRight = -1; //-1 = left, 1=right
const int maxDrawTime = 1000; //max time for each line in millisecs.
//this corresponds to the max amplitude of the line
//vars for buttons
int leftState = 0;
int rightState = 0;
int start = -1;
int startStatus = 0;
//vars for PING)))
long pingTime = 0.0;
int inchDistance = 0;
void setup(){
myservo.attach(9); //attaches the servo to the pwm in pin 9
pinMode(leftButton, INPUT); //initialize the push button as inputs
pinMode(rightButton, INPUT);
pinMode(stopButton, INPUT);
//debug stuff
Serial.begin(9600);
}
void loop(){
leftState = digitalRead(leftButton);
rightState = digitalRead(rightButton);
startStatus = digitalRead(stopButton);
if(startStatus == HIGH)
start = start*-1;
//Do normal routine of checking distance and what not
if( start > 0 ){
//debugggg life
Serial.println(“in normal”);
//Pulse out
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
//read pulse back
pinMode(pingPin, INPUT);
pingTime = pulseIn(pingPin, HIGH);
//convert that s**t to inches and cast it as an int yeah! and constrain it and then map it
inchDistance = constrain( (int) timeToInches(pingTime), minDist, maxDist);
inchDistance = map(inchDistance, minDist, maxDist, maxDrawTime, 0);
inchDistance = inchDistance – inchDistance%100;
//More debug things
Serial.println(inchDistance);
if(inchDistance >= 100){
//debug stuff dont pay attention
Serial.println(“i shouldnt be here!”);
//move those motors
moveMotors(inchDistance);
delay(100);
}
}
//manual movements using the buttons used for calibration
else if ( start < 0){
//More debug, not important
Serial.println(“in calib”);
Serial.println(start);
//turn left
if(leftState == HIGH && rightState == LOW){
myservo.writeMicroseconds(1300); //turn motor clockwise
delay(100);
myservo.writeMicroseconds(1500); //stop
//Deeeebug stuff!
Serial.println(“left”);
}
//turn right
else if(leftState == LOW && rightState == HIGH){
myservo.writeMicroseconds(1700); //turn motor counter-clockwise
delay(100);
myservo.writeMicroseconds(1500); //stop
//deeeeeeeeeebug!
Serial.println(“right”);
}
}
}//end of loop()
//taken directly from the Ping example on:http://www.arduino.cc/en/Tutorial/Ping
//because it makes my life esier
long timeToInches(long microsecs){
// According to Parallax’s datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
return microsecs / 74 / 2;
}
void moveMotors(int time){
// go left
if(leftOrRight < 0){
myservo.writeMicroseconds(1300); //turn motor clockwise
delay(time);
myservo.writeMicroseconds(1500); //stop
//return to original pos
myservo.writeMicroseconds(1700); //turn motor counter-clockwise
delay(time);
myservo.writeMicroseconds(1500); //stop
}
//go right
else if(leftOrRight > 0){
myservo.writeMicroseconds(1700); //turn motor counter-clockwise
delay(time);
myservo.writeMicroseconds(1500); //stop
//return to original position
myservo.writeMicroseconds(1300); //turn motor clockwise
delay(time);
myservo.writeMicroseconds(1500); //stop
}
//update leftOrRight
leftOrRight = leftOrRight * -1;
//More debug, not important
Serial.println(leftOrRight);
Serial.println(time);
}