0

Twin DC Motor Control using Ultrasonic Sensor

Looking for help trying to get code built to operate two DC motors. I have the code for one DC motor.

int encoderPin = 4;
int direction = 5;
int encoderPos = 0;
int p0, p1;
unsigned long t1, t2;
int cm = 0;

void setup() {
pinMode (encoderPin, INPUT);
pinMode (direction, INPUT);
attachInterrupt(0, EncoderCount, RISING);
Serial.begin (115200);
t1 = millis();
}

void loop() {
p0 = p1;
p1 = encoderPos;
t1 = t2;
t2 = millis();
cm = 0.01723 * readUltrasonicDistance(7, 7);

Serial.print(p1);
Serial.print(" ");
Serial.print(String((float)1000*(p1-p0)/(t2-t1)));
Serial.print(" ");
Serial.print(t2-t1);
Serial.print(" ");
Serial.print(cm);
Serial.print(" cm\n");
if(cm > 60)
{
analogWrite(10, 0);
analogWrite(11, 100);
}
if(cm > 15 && cm < 30)
{
analogWrite(10, 0);
analogWrite(11, 30);
}
if(cm < 15)
{
analogWrite(10, 0);
analogWrite(11, 0);
}

delay(10);
}

void EncoderCount() {

if (digitalRead(direction)==HIGH)
encoderPos = encoderPos - 1;
else
encoderPos = encoderPos + 1;

}

long readUltrasonicDistance(int triggerPin, int echoPin)
{
pinMode(triggerPin, OUTPUT); // Clear the trigger
digitalWrite(triggerPin, LOW);
delayMicroseconds(2);
// Sets the trigger pin to HIGH state for 10 microseconds
digitalWrite(triggerPin, HIGH);
delayMicroseconds(10);
digitalWrite(triggerPin, LOW);
pinMode(echoPin, INPUT);
// Reads the echo pin, and returns the sound wave travel time in microseconds
return pulseIn(echoPin, HIGH);
}

0 comments

Please sign in to leave a comment.