Hello everyone..
I am having difficulties to correctly make the code for my project. The idea is that when you detect an obstacle at 20cm, turn 90 degrees and re-start the march, but I do not know why the distance sensor code and the servomotor code that moves the sensor from side to side creates some kind of conflict since they begin to make noise the drivers of the motors, but these do not move .. If I leave only the code of the stepper motors if they turn without problems .. although I have not known to do the part of that when it detects an object, turn 90 degrees...
This is the code that I am using at the moment:
#include <Servo.h>
Servo servo1;
//Motor1
int PUL = 4; // define pin pulse
int DIR = 3; // define Direction pin
int ENA = 2; // define Enable Pin
//Motor2
int PUL2 = 7; // define pin pulse
int DIR2 = 6; // define Direction pin
int ENA2 = 5; // define Enable Pin
#include "Ultrasonic.h"
int trigPin = 8;
int echoPin = 9;
Ultrasonic ultrasound (trigPin, echoPin);
int dis = 20;
int distance;
int pinServo = 10; // Digital pin with PWM for the Servo
int pulseMin = 650; // Pulse in us to rotate an angle of 0º
int pulseMax = 2550; // Pulse in us to rotate a 180º angle
int angle = 0; // Variable to save the angle of the servo
int from = 0;
int to = 120;
int step = 1;
int ledrele = 11;
void setup () {
servo1.attach (pinServo, pulseMin, pulseMax);
pinMode (trigPin, OUTPUT);
pinMode (echoPin, INPUT);
pinMode (PUL, OUTPUT);
pinMode (DIR, OUTPUT);
pinMode (ENA, OUTPUT);
pinMode (PUL2, OUTPUT);
pinMode (DIR2, OUTPUT);
pinMode (ENA2, OUTPUT);
pinMode (ledrele, OUTPUT);
digitalWrite (ledrele, LOW);
}
void loop () {
angle + = step;
servo1.write (angle);
delay (15);
if (angle> = 120) {
step = -1;
from = 120;
up = 0;
}
if (angle == 0) {
step = 1;
from = 0;
to = 120;
}
distance = ultrasound.Ranging (CM);
if (distance <dis) {
advance();
} else {
stop();
}
}
void advance() {
digitalWrite (DIR, HIGH);
digitalWrite (ENA, HIGH);
digitalWrite (PUL, HIGH);
delayMicroseconds (50);
digitalWrite (PUL, LOW);
delayMicroseconds (50);
digitalWrite (DIR2, LOW);
digitalWrite (ENA2, HIGH);
digitalWrite (PUL2, HIGH);
delayMicroseconds (50);
digitalWrite (PUL2, LOW);
delayMicroseconds (50);
}
void stop () {
digitalWrite (DIR, LOW);
digitalWrite (ENA, HIGH);
digitalWrite (PUL, HIGH);
delayMicroseconds (50);
digitalWrite (PUL, LOW);
delayMicroseconds (50);
digitalWrite (DIR2, HIGH);
digitalWrite (ENA2, HIGH);
digitalWrite (PUL2, HIGH);
delayMicroseconds (50);
digitalWrite (PUL2, LOW);
delayMicroseconds (50);
}
This is the diagram I am using for stepper motors and TB6600 drivers:
And some images of the robot:
Sorry for my english, I use a translator. Thanks for the help.
