I've been trying to make a line-following / object-avoiding robot but I'm stuck due to a bug in the ultrasonic sensor code. I don't think it's the code itself. I think it's because of some other part of the code affecting it because I tested it with only the ultrasonic sensor code and it worked great.
What I need from the code:
I need the distance in cm and to return the distance all the time.
#include <NewPing.h>
#include <Servo.h>
#define MOTOR_SPEED 200
#define ADJ_MOTOR_SPEED 75
Servo ser1;
int servoPin = 11;
int servoPos1 = 0;
int servoPos2 = 90;
int servoPos3 = 180;
int irL = 2;
int irR = 3;
int Lm1 = 8; //Left motor 1
int enB = 6; //Enable left motor
int Lm2 = 7; //Left motor 2
int Rm1 = 10; //Right motor 1
int enA = 5; //Enable Right motor
int Rm2 = 9; //Right motor 2
int trigPin = 13;
int echoPin = 12;
int uSenDis = 200; // Sets ultra sonic sensor distance to 20cm
int mxDisOb = 20; // Max distance with an object
NewPing sonar(trigPin,echoPin,uSenDis);
int distance;
bool Leftclear;
bool Rightclear;
int dt1 = 1000;
int dt2 = 100;
int dt3 = 200;
int dt4 = 500;
void setup() {
// put your setup code here, to run once:
TCCR0B = TCCR0B & B11111000 | B00000010 ;
Serial.begin(9600);
ser1.attach (servoPin);
pinMode(servoPin,OUTPUT);
pinMode(irL,INPUT);
pinMode(irR,INPUT);
pinMode(Rm1,OUTPUT);
pinMode(Rm2,OUTPUT);
pinMode(enA,OUTPUT);
pinMode(Lm1,OUTPUT);
pinMode(Lm2,OUTPUT);
pinMode(Lm2,OUTPUT);
pinMode(trigPin,OUTPUT);
pinMode(echoPin,INPUT);
rotateM(0,0);
}
void loop() {
// put your main code here, to run repeatedly
for (int j=1; j<=1; j=j+1){
ser1.write(servoPos2); // Turn servo motor towards the forward direction (90 degrees)
int RirV = digitalRead(irR); // Read the value from Right IR sensor
int LirV = digitalRead(irL); // Read the value from Left IR sensor
}
delay(dt4);
distance = sonar.ping_cm();
Serial.println(distance);
/*if (distance < mxDisOb && distance > 1){
checkRight();
if (Rightclear == true){
stop();
delay(dt3);
rightTurn();
}
else if(Leftclear == true) {
stop();
delay(dt3);
leftTurn();
}
}*/
//rotateM (MOTOR_SPEED,MOTOR_SPEED);
/*// If neither of the sensors detect black line, Move forward
if (RirV == LOW && LirV == LOW){
rotateM (MOTOT_SPEED,MOTOR_SPEED);
}
// If right sensor detects the black line, turn right
else if (RirV == HIGH && LirV == LOW){
rotateM (-MOTOT_SPEED,MOTOR_SPEED);
}
// If left sensor detects the black line, turn left
else if (RirV == LOW && LirV == HIGH){
rotateM (MOTOT_SPEED,-MOTOR_SPEED);
}
// IF both sensors detect black line, Stop
else {
rotateM(0,0);
}
*/
//rotateM (MOTOR_SPEED,MOTOR_SPEED);
}
void rotateM(int rightMotorSpeed, int leftMotorSpeed)
{
if (rightMotorSpeed < 0)
{
digitalWrite(Rm1,LOW);
digitalWrite(Rm2,HIGH);
}
else if (rightMotorSpeed > 0)
{
digitalWrite(Rm1,HIGH);
digitalWrite(Rm2,LOW);
}
else
{
digitalWrite(Rm1,LOW);
digitalWrite(Rm2,LOW);
}
if (leftMotorSpeed < 0)
{
digitalWrite(Lm1,LOW);
digitalWrite(Lm2,HIGH);
}
else if (leftMotorSpeed > 0)
{
digitalWrite(Lm1,HIGH);
digitalWrite(Lm2,LOW);
}
else
{
digitalWrite(Lm1,LOW);
digitalWrite(Lm2,LOW);
}
analogWrite(enA, abs(rightMotorSpeed));
analogWrite(enB, abs(leftMotorSpeed));
}
void rightTurn(){
rotateM(-ADJ_MOTOR_SPEED, ADJ_MOTOR_SPEED);
delay(dt3);
}
void leftTurn(){
rotateM(ADJ_MOTOR_SPEED, -ADJ_MOTOR_SPEED);
delay(dt3);
}
void checkRight(){
ser1.write(servoPos1); // Turn servo motor to the right (0 degrees)
distance = sonar.ping_cm();
delay(dt2);
if (distance > 50 && distance < 1){
checkLeft();
}
else{
Rightclear = true;
}
Rightclear = false;
}
void checkLeft(){
ser1.write(servoPos3); // Turn servo motor to the Left (180 degrees)
distance = sonar.ping_cm();
delay(dt2);
if (distance > 50 && distance < 1){
stop();
}
else{
Leftclear = true;
}
Leftclear = false;
}
void USsenVal(){
delay(dt4);
distance = sonar.ping_cm();
Serial.println(sonar.ping_cm());
}
void stop(){
rotateM(0, 0);
ser1.write(servoPos2);
}