Hello, I have been building a collision avoidance vehicle with a DIY sensor on the front of the vehicle and a kit sensor mounted on a servomotor at the back of the vehicle. These work fine, and will override the drive commands if an object is in the way. However, the drive of the vehicle it not so good, with it being jumpy.... I believe to be because of the delays (with the servo function). Can anyone suggest, how can I get around this, is there anyway to run two functions at the same time?
Any help will be welcomed.
#include <Servo.h>
Servo scanservo; //Ping Sensor Servo
#define txPin 4
#define rxPin A4
const int motor1 = 12;
const int motor2 = 9;
const int steer1 = 13;
const int steer2 = 8;
const int PWM_M = 3;
int up = 0;
int down = 0;
int left = 0;
int right = 0;
const int a = 800;
const int turntime = 500; //Number of milliseconds to turn when turning
const int pingPin = 6; //Pin that the Ping sensor is attached to.
const int echoPin = 5; //Pin that the Ping sensor is attached to.
const int scanservopin = 7; // Pin number for scan servo
const int distancelimit = 30;
void setup()
{
Serial.begin (9600);
scanservo.attach(scanservopin); // Attach the scan servo
pinMode(echoPin, INPUT);
pinMode(rxPin, INPUT);
pinMode(txPin, OUTPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(steer1, OUTPUT);
pinMode(steer2, OUTPUT);
digitalWrite(motor1,LOW);
digitalWrite(motor2,LOW);
digitalWrite(steer1,LOW);
digitalWrite(steer2,LOW);
}
void loop(){
left = analogRead(A0);
Serial.print("left: ");
Serial.println(left);
right = analogRead(A1);
Serial.print("right: ");
Serial.println(right);
down = analogRead(A3);
Serial.print("down: ");
Serial.println(down);
up = analogRead(A2);
Serial.print("up: ");
Serial.println(up);
int sensor = pingforward();
Serial.print("sensor: ");
Serial.println(sensor);
int leftscanval = scanleft();
Serial.print("leftscanval: ");
Serial.println(leftscanval);
int rightscanval = scanright();
Serial.print("rightscanval: ");
Serial.println(rightscanval);
int centerscanval = scancenter();
Serial.print("centrescanval: ");
Serial.println(centerscanval);
if (down < 400 && centerscanval > distancelimit){
backward(1);
}
if (up < 400 && sensor > 150){
forward(1);
}
if (left > 400 && leftscanval > distancelimit){
turnleft(1);
}
if (right > 400 && rightscanval > distancelimit){
turnright(1);
}
if (down > 400 && up > 400 && left < 400 && right < 400){
stopmotors(1);
}
}
void turnleft(int t){
digitalWrite (13, HIGH);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delayMicroseconds(t);
}
void turnright(int t){
digitalWrite (13, LOW);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delayMicroseconds(t);
}
void stopmotors(int t){
digitalWrite (9, LOW);
digitalWrite (8, LOW);
digitalWrite (3, LOW);
digitalWrite (11, LOW);
delayMicroseconds(t);
}
void forward(int t){
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(PWM_M, HIGH);
delay(t);
}
void backward(int t){
digitalWrite (9, LOW);
digitalWrite (12, LOW);
digitalWrite(PWM_M, HIGH);
delay(t);
}
int ping(){
long distance, duration;
//Send Pulse
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
//Read Echo
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
distance=(duration/2)*0.0341;
return distance;
}
int pingforward(){
long senseforward;
for (int i = 0; i < 8; i++) {
digitalWrite(txPin, LOW); // this sends out a short pulse for 10ms
delayMicroseconds(9);
digitalWrite(txPin, HIGH);
delayMicroseconds(9);
}
digitalWrite(txPin, LOW);
delay(2);
senseforward = analogRead(A4); // us the ping() function to see if anything is ahead.
return senseforward;
}
char scanleft(){
long leftscanval;
scanservo.write(30);
delay(1000);
leftscanval = ping();
return leftscanval;
}
char scanright(){
long rightscanval;
scanservo.write(150);
delay(1000);
rightscanval = ping();
return rightscanval;
}
char scancenter(){
long centerscanval;
scanservo.write(90);
delay(1000);
centerscanval = ping();
return centerscanval;
}