hello.I have a robot (2 motors),I have 4 ultrasonic sensors(front ,right ,left and back) and I use newPing library,
but when the distance of front sensor is smaller than 15 cm,I want motors to stop,but the 2 motors rotates
(left motor rotates backward and right motor rotates forward)...
Here is the code for what I mean:
#include <NewPing.h>
const int IN_1_left = 32;
const int IN_2_left = 33;
const int PWM_left = 7;
const int IN_1_right = 36;
const int IN_2_right = 37;
const int PWM_right = 6;
unsigned long presentMillis = 0;
boolean enable_forward = true;
boolean enable_stop = false;
boolean enable_rotate_AUTO = false;
#define SONAR_NUM 4 // Number of sensors.
#define MAX_DISTANCE 400 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned long cm[SONAR_NUM]; // Where the ping distances are stored.
uint8_t currentSensor = 0; // Keeps track of which sensor is active.
NewPing sonar[SONAR_NUM] = // Sensor object array. Each sensor's trigger pin, echo pin, and max distance to ping.
{
NewPing(24, 25, MAX_DISTANCE), // front sensor
NewPing(27, 26, MAX_DISTANCE), // right sensor
NewPing(28, 29, MAX_DISTANCE), // left sensor
NewPing(22, 23, MAX_DISTANCE) // back sensor
};
void setup()
{
Serial.begin(9600);
for (uint8_t i = 1; i < 4; i++) // Set the starting time for each sensor.
{
pingTimer[i] = pingTimer[i - 1] + 33;
}
analogWrite(PWM_left, 0);
analogWrite(PWM_right, 0);
pinMode(PWM_left, OUTPUT);
pinMode(PWM_right, OUTPUT);
pinMode(IN_1_left, OUTPUT);
pinMode(IN_2_left, OUTPUT);
pinMode(IN_1_right, OUTPUT);
pinMode(IN_2_right, OUTPUT);
}
void echoCheck() // If ping received, set the sensor distance to array.
{
if (sonar[currentSensor].check_timer())
{
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}
}
void ultrasonic_sensors()
{
for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
if (millis() >= pingTimer[i]) { // Is it this sensor's time to ping?
pingTimer[i] += PING_INTERVAL * SONAR_NUM; // Set next time this sensor will be pinged.
if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
sonar[currentSensor].timer_stop(); // Make sure previous timer is canceled before starting a new ping (insurance).
currentSensor = i; // Sensor being accessed.
cm[currentSensor] = 0; // Make distance zero in case there's no ping echo for this sensor.
sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
}
}
}
void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
// The following code would be replaced with your code that does something with the ping results.
for (uint8_t i = 0; i < SONAR_NUM; i++) {
Serial.print(i + 1);
Serial.print(" = ");
Serial.print(cm[i]);
Serial.print("cm ");
}
Serial.println();
}
void Autopilot()
{
unsigned long currentMillis = millis();
if (enable_forward)
{
if (currentMillis - presentMillis >= 330) // ΜΠΡΟΣΤΑ
{
FORWARD();
enable_forward = false;
enable_stop = true;
presentMillis = currentMillis;
}
}
if (enable_stop && cm[0] <= 15)
{
if (currentMillis - presentMillis >= 10) //ΘΑ ΣΤΑΜΑΤΗΣΕΙ (STOP) ΓΙΑ 400 ms ...
{
STOP();
enable_stop = false;
enable_rotate_AUTO = true;
presentMillis = currentMillis;
}
}
if (enable_rotate_AUTO) //ΘΑ ΣΤΡΙΨΕΙ ΔΕΞΙΑ (RIGHT) 'Η ΑΡΙΣΤΕΡΑ (LEFT),--ΑΝΑΛΟΓΑ ΤΗΝ ΕΝΔΕΙΞΗ ΤΟΥ ΑΙΣΘΗΤΗΡΑ-- ΚΑΙ ΘΑ ΣΤΡΙΨΕΙ ΓΙΑ 330 ms ...
{
if (currentMillis - presentMillis >= 400)
{
if (cm[1] > cm[2])
{
RIGHT();
enable_forward = true;
enable_rotate_AUTO = false;
}
if (cm[2] > cm[1])
{
LEFT();
enable_forward = true;
enable_rotate_AUTO = false;
}
presentMillis = currentMillis;
}
}
}
void FORWARD()
{
analogWrite(PWM_left, 250);
analogWrite(PWM_right, 255);
digitalWrite(IN_1_left , LOW); //forward
digitalWrite(IN_2_left , HIGH);
digitalWrite(IN_1_right , LOW);
digitalWrite(IN_2_right , HIGH);
}
void BACKWARD()
{
analogWrite(PWM_left, 250);
analogWrite(PWM_right, 255);
digitalWrite(IN_1_left , HIGH); //backward
digitalWrite(IN_2_left , LOW);
digitalWrite(IN_1_right , HIGH);
digitalWrite(IN_2_right , LOW);
}
void STOP()
{
digitalWrite(IN_1_left , LOW); //stop
digitalWrite(IN_2_left , LOW);
digitalWrite(IN_1_right , LOW);
digitalWrite(IN_2_right , LOW);
}
void RIGHT()
{
analogWrite(PWM_left, 255);
analogWrite(PWM_right, 255);
digitalWrite(IN_1_left , LOW); //right
digitalWrite(IN_2_left , HIGH);
digitalWrite(IN_1_right , HIGH);
digitalWrite(IN_2_right , LOW);
}
void LEFT()
{
analogWrite(PWM_left, 255);
analogWrite(PWM_right, 255);
digitalWrite(IN_1_left , HIGH); //left
digitalWrite(IN_2_left , LOW);
digitalWrite(IN_1_right , LOW);
digitalWrite(IN_2_right , HIGH);
}
void loop()
{
Autopilot();
ultrasonic_sensors();
}
I tested the distances of 4 sensors and works fine,but I have the problem of 2 motors...
DO you know where the problem is?
Thanks...