I have an RC car with an Arduino Mega. I have three ultra-sonic sensors connected. Everything is talking to each other.
Using motor and NewPing libraries
Im really just looking to see if there are more efficient ways to writing this sketch. It is a working progress, I tried to explain what all is going on.
Any advice an or ideas would be greatly appreciated.
This is the whole thing:
// ---------------------------------------------------------------------------
// This example code was used to successfully communicate with 15 ultrasonic sensors. You can adjust
// the number of sensors in your project by changing SONAR_NUM and the number of NewPing objects in the
// "sonar" array. You also need to change the pins for each sensor for the NewPing objects. Each sensor
// is pinged at 33ms intervals. So, one cycle of all sensors takes 495ms (33 * 15 = 495ms). The results
// are sent to the "oneSensorCycle" function which currently just displays the distance data. Your project
// would normally process the sensor results in this function (for example, decide if a robot needs to
// turn and call the turn function). Keep in mind this example is event-driven. Your complete sketch needs
// to be written so there's no "delay" commands and the loop() cycles at faster than a 33ms rate. If other
// processes take longer than 33ms, you'll need to increase PING_INTERVAL so it doesn't get behind.
// ---------------------------------------------------------------------------
#include <NewPing.h>
#include <AFMotor.h>
AF_DCMotor motor(2, MOTOR12_2KHZ); // create motor #2, max 64KHz
AF_DCMotor steering(1, MOTOR12_2KHZ); // create steering motor #1, max 64KHz
#define SONAR_NUM 3 // Number or sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 34 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
byte botSpeed = 80; // Sets initial speed to 80
byte botDir = 150; // Sets speed for steering motor
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int 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(53, 52, MAX_DISTANCE), // LEFT Sensor
NewPing(51, 50, MAX_DISTANCE), // FRONT Sensor
NewPing(49, 48, MAX_DISTANCE) // RIGHT Sensor
};
void setup() {
Serial.begin(115200);
motor.setSpeed(botSpeed); // set the speed to 80/255
steering.setSpeed(botDir); // set stering motor speed to 200
pingTimer[0] = millis() + 75; // First ping starts at 75ms, gives time for the Arduino to chill before starting.
for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}
void loop() {
motor.setSpeed(botSpeed); //Think this will make sure the speed is better controlled (just felt like it was needed)
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).
}
}
// The rest of your code would go here.
}
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 goForward()
{
motor.run(BACKWARD);
}
void goBackwards()
{
motor.run(FORWARD);
}
void Coast()
{
motor.run(RELEASE);
}
void goLeft()
{
steering.run(BACKWARD);
}
void goRight()
{
steering.run(FORWARD);
}
void speedUp()
{
float Sinc = 1.1;
botSpeed = min(botSpeed, 130); //sets maximum speed to 130
botSpeed += Sinc; //adds 1.1 to current speed
motor.setSpeed(botSpeed); //Sets new speed
// Serial.println(botSpeed); //used to check it is working properly(serves no other purpose) will comment out asap
}
void speedDown()
{
float Sdec = .96;
botSpeed = max(botSpeed, 84); //Sets minimum speed to 79
botSpeed *= Sdec; //Multiplies .95 by current speed
motor.setSpeed(botSpeed); //Sets new speed
//Serial.println(botSpeed); //used to check it is working properly(serves no other purpose) will comment out asap
}
void speedDown2()
{
float Sdec2 = .90;
botSpeed = max(botSpeed, 84); //Sets minimum speed to 79
botSpeed *= Sdec2; //Multiplies .90 by current speed
motor.setSpeed(botSpeed); //Sets new speed
}
void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
for (uint8_t i = 0; i < SONAR_NUM; i++) {
Serial.print(i);
Serial.print("=");
Serial.print(cm[i]);
Serial.print("cm ");
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
////////// Is there a better way to write this? |
// |
// V ?
if (cm[1] > 0)
{
if(cm[1] < 50) //If distance is between 1-49 go backwards
{
goBackwards(); //backwards function
}
else
{
if(cm[1] < 80) //if distance is between 50-79 Slow down faster!!!
{
speedDown2();
goForward();
}
else
{
if(cm[1] < 200) //If distance is between 80-199 slow down
{
speedDown(); //Slow down function
goForward(); //Forward function
}
}
}
}
else //If distance is 0, there is nothing in the way
{
goForward();
speedUp();
}
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
// Havent really gotten to the turning part yet ///////////
////////// [0] = left sensor
////////// [2] = right sensor
if (cm[0] > cm[2] )
{
//Serial.print("go Left");
goLeft();
}
if (cm[2] > cm[0] )
{
//Serial.print("go Right");
goRight();
}
}
Serial.println();
}