Hello
Can someone tell me how to better control motor speed in my robot code?
it's working but I wanna make it autonomous and it needs to regulate speed by itself.
Hardware:
Arduino mega
L298N Motor Driver shield
ultrasonic sensors
dc motor (interactive R2D2)
// ---------------------------------------------------------------------------
// 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>
#define SONAR_NUM 4 // Number of sensors.
#define MAX_DISTANCE 300 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 50 // 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 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.
NewPing(26, 27, MAX_DISTANCE), // FRONT SENSOR Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(28, 29, MAX_DISTANCE), // DX SENSOR
NewPing(24, 25, MAX_DISTANCE), //LX SENSOR
NewPing(22, 23, MAX_DISTANCE), // BACK MSENSOR
// NewPing(23, 24, MAX_DISTANCE),
// NewPing(25, 26, MAX_DISTANCE),
// NewPing(27, 28, MAX_DISTANCE),
// NewPing(29, 30, MAX_DISTANCE),
// NewPing(31, 32, MAX_DISTANCE),
// NewPing(34, 33, MAX_DISTANCE),
// NewPing(35, 36, MAX_DISTANCE),
// NewPing(37, 38, MAX_DISTANCE),
// NewPing(39, 40, MAX_DISTANCE),
// NewPing(50, 51, MAX_DISTANCE),
// NewPing(52, 53, MAX_DISTANCE)
};
//Motor LX;
int dir1PinA = 13;
int dir2PinA = 12;
int speedPinA = 10;
//motor RX;
int dir1PinB = 11;
int dir2PinB = 8;
int speedPinB = 9;
//int spead = 255;//define the spead of motor
void setup() {
Serial.begin(9600);
pinMode (dir1PinA, OUTPUT);
pinMode (dir2PinA, OUTPUT);
pinMode (speedPinA, OUTPUT);
pinMode (dir1PinB, OUTPUT);
pinMode (dir2PinB, OUTPUT);
pinMode (speedPinB, OUTPUT);
// -------------PING---------------------
pingTimer[0] = millis() + 50; // 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;
//----------------motors functions
}
void forward()
{
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, HIGH);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, HIGH);
}
void backward()
{
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
digitalWrite (dir1PinA, HIGH);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, HIGH);
digitalWrite (dir2PinB, LOW);
}
void left()
{
analogWrite(speedPinA, 180);
analogWrite(speedPinB, 0);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, HIGH);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, LOW);
}
void right()
{
analogWrite(speedPinA, 0);
analogWrite(speedPinB, 180);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, HIGH);
}
void stop()
{
analogWrite(speedPinA, 0);
analogWrite(speedPinB, 0);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, LOW);
}
//-----------------end motors function------------------------
void loop() {
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).
}
}
cm[0] = constrain(cm[0], 5, 300);
cm[1] = constrain(cm[1], 5, 300);
cm[2] = constrain(cm[2], 5, 300);
cm[3] = constrain(cm[3], 5, 300);
// Other code that *DOESN'T* analyze ping results can 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 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);
Serial.print("=");
Serial.print(cm[i]);
Serial.print("cm ");
}
Serial.println();
// GO forward
if (cm[0] >= 30) {
// -----------------------------------Center sensor
//stop();
forward();
Serial.println("FWD A");
Serial.println("FWD B");
}
if (cm[0] <= 30 && cm[0] != 5) {
stop();
delay(100);
backward();
Serial.println("BKW A");
Serial.println("BKW B");
//right();
}
//---------------------------------------------RIGHT SENSOR
if (cm[1] <= 20 && cm[1] != 5) {
left();
// MOTORS TURN RIGH M
Serial.println("FWD RX A"); //RX MOTOR ON
Serial.println("STOP B"); //LX MOTOR OFF
}
if (cm[2] <= 20 && cm[2] != 5) { //---------------- LEFT SENSOR
right();
Serial.println("STOP A");// RX MOTOR OFF
Serial.println("FWD B");// LX MOTOR ON
}
if (cm[3] <= 20 && cm[3] != 5) { // -------------------BACK SENSOR
stop();
delay (100);
forward();
Serial.println("STOP A");
Serial.println("STOP A");
Serial.println("STOP B");
Serial.println("STOP B");
//right();
}
//----------------------STUCK BACK RIGHT
/*
if (cm[3] <=10 && cm[1] <=10) {
}
//------------------------STUCK BACK LEFT
if (cm[3] <=10 && cm[2] <=10) {
if (dir1PinA = HIGH)
Serial.println("BKW A");
if (dir1PinB = LOW)
Serial.println("STOP B");
}
*/
}
Thanks