I'm building a 4wd car, with an Arduino Uno, Sensor Shield v5 and an L298N module to drive the wheel motors.
To set the speed - analogWrite (speedPinA, spdR); // spdR varies from 0 - 160
to set the direction of the motors digitalWrite (dir1PinA, MAp1) // MAp1 is HIGH or LOW
speedPins are d9 and d10
dirPins are d2,d4,d5 and d7
all works great... I can control the speed and the wheel direction... now I wanted to add obstacle detection
I added the HC-SR04 sonic sensor with the trigger on Pin D13, echo on Pin D12 - works fine.
now the issue - I want to use a single servo to control the direction of a HC-SR04. I wired the signal to pin D11 and added the 'servo.h' and needed code to control the servo. with that code added I've lost the motor controls - speed and direction, the sonic sensor still works
I've isolated it to this line of code -
myservo.attach(11); // servo control pin on D11
if I let it execute then I lose the commands to the wheels; comment that line out and I lose the
servo controls, but get the wheels back....
moving the servo control to pin D8 has the same issue...
/CODE
#include <Servo.h>
Servo myservo;
const unsigned int SERVO_PIN=8; // 8th pin on servo 5 board for sonic 'servo' motor
const unsigned int TRIG_PIN=13; // 13th pin on servo 5 board for sonic sensor trigger
const unsigned int ECHO_PIN=12; // 12th pin on servo 5 board for sonic sensor echo
const unsigned int BAUD_RATE=9600;
const unsigned int SERVO_LEFT = 1900;
const unsigned int SERVO_RIGHT = 500;
const unsigned int SERVO_MID = (SERVO_LEFT - SERVO_RIGHT)/2 + SERVO_RIGHT;
int distance = 0;
int dist_R = 0;
int dist_L = 0;
//Motor A
int dir1PinA = 7;
int dir2PinA = 5;
int speedPinA = 10;
//motor B
int dir1PinB = 2;
int dir2PinB = 4;
int speedPinB = 9;
unsigned long time;
int speedR = 125; // use different speed values for left and right to compensate for drift
int speedL = 160; // probably caused by slightly different motor speeds, tire slips etc...
int dir = 0;
// Each motor uses 2 pins to control the direction. Pin 1 and Pin 2 are always set opposite
// if one is HIGH, the other MUST BE LOW.
// 4/17/20 MotorA and Motor B on now oriented the same physical direction
// therefore HIGH, LOW, HIGH, LOW equals backwards and
// LOW, HIGH, LOW, HIGH equals forwards
// HIGH, LOW, LOW, HIGH equals left turn
// LOW, HIGH, HIGH, LOW equals right turn
void set_direction(bool MAp1, bool MAp2, bool MBp1, bool MBp2)
{
digitalWrite (dir1PinA, MAp1);
digitalWrite (dir2PinA, MAp2);
digitalWrite (dir1PinB, MBp1);
digitalWrite (dir2PinB, MBp2);
}
void go_forward(int spdL, int spdR)
{
// get the car moving forward at the indicated speed
analogWrite (speedPinA, spdR); // Right wheels
analogWrite (speedPinB, spdL); // left wheels
set_direction(LOW, HIGH, LOW, HIGH);
}
void go_backward(int spdL, int spdR)
{
set_direction(HIGH, LOW, HIGH, LOW);
analogWrite (speedPinA, spdR); // Right wheels
analogWrite (speedPinB, spdL); // left wheels
}
void go_right(int spdL, int spdR)
{
set_direction(LOW, HIGH, HIGH, LOW);
analogWrite (speedPinA, spdR); // Right wheels
analogWrite (speedPinB, spdL); // left wheels
}
void go_left(int spdL, int spdR)
{
set_direction(HIGH, LOW, LOW, HIGH);
analogWrite (speedPinA, spdR); // Right wheels
analogWrite (speedPinB, spdL); // left wheels
}
void slow_or_stop_car(int spdL, int spdR)
{
analogWrite (speedPinA, spdR); // Right wheels
analogWrite (speedPinB, spdL); // left wheels
}
int get_distance()
{
int distance[] = {0,0,0};
int cms = 0;
unsigned long duration= 0;
int count = 0;
// this is the number of times to read the sonic sensor before returning distance
// inconsistent readings are possible based on environment, electronic errors etc
while (count < 3) {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
duration = pulseIn(ECHO_PIN, HIGH);
distance[count] = duration/58; // divide time by 58 for cm or 148 for inches
/*
if(duration==0){
Serial.println("Warning: no pulse from sensor");
}
else{
Serial.print("distance to nearest object:");
Serial.print(distance[count]);
Serial.println(" cm");
}
*/
++count;
delay(100);
}
// now just take the values in the array total them and divide by count -
// this will eliminate the random small values that
// cause the car to stop, the larger values are ok as we just want to continue
cms = ((distance[0] + distance[1] + distance[2])/count);
/* Serial.print(" returning this distance ");
Serial.print (cms) ;
Serial.println(" cm");
*/
return (cms);
}
void turn_sonic (int angle)
{
myservo.writeMicroseconds (angle);
}
void setup ()
{
Serial.begin(BAUD_RATE);
pinMode (dir1PinA, OUTPUT);
pinMode (dir2PinA, OUTPUT);
pinMode (speedPinA, OUTPUT);
pinMode (dir1PinB, OUTPUT);
pinMode (dir2PinB, OUTPUT);
pinMode (speedPinB, OUTPUT);
time = millis ();
myservo.attach(SERVO_PIN); // control pin for sonic sensor servo
myservo.writeMicroseconds (SERVO_MID);
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
distance = get_distance();
Serial.println("Setup complete - here we go");
}
void loop () {
distance = get_distance();
Serial.print("Distance = ");
Serial.println(distance);
if(distance >= 30)
{
go_forward (speedL, speedR);
Serial.println(" going forward");
}
else if (distance > 10 && distance < 30)
{
slow_or_stop_car(speedL - 50, speedR - 50); // slow the speed by half
Serial.println("forward but slower");
}
else
/*
- here is where the action will take place. when the car finds an obstacle it will stop, look right,
- then look left to determine which is the more open path. It will then issue the commands to turn
- right or left depending, recenter the sensor and go forward...
*/
{
slow_or_stop_car (0,0);
Serial.println("STOP");
delay(500);
turn_sonic(SERVO_RIGHT);
dist_R = get_distance();
turn_sonic(SERVO_LEFT);
dist_L = get_distance();
delay(500);
turn_sonic(SERVO_MID);
if (dist_R > dist_L)
{
go_right(speedL -50, speedR - 50);
}
else
{
go_left(speedL - 50, speedR - 50);
}
/*
go_backward (speedL - 50, speedR - 50);
Serial.println("go backward");
delay(1000);
*/
}
delay(500);
}