Hi I am new to Arduino and top of that have decided to allow my 17 year old students build an ultrasonic 2wd vehicle! We have all successfully built and coded the robot, however, when we try and reduce the speed, only one motor will work. The board is a Diecimila and L298 motor board. I hope someone can help ![]()
/*
Ultrasonic Vehicle
GeekStudio
http://www.geeker.co.nz
L = Left
R = Right
F = Front
B = Back
*/
#include <Servo.h>
int pinLB =11; // Pin6 rb+
int pinLF = 10; // Pin6 rf
int pinRB = 9;// Pin6 RB+0.
int pinRF = 6; // Pin6 RF
int motorA = A2;
int motorB = A3;
int inputPin = A0; // Connect to Ultrasonic Sensor Echo Pin
int outputPin =A1; // Connect to Ultrasonic Sensor Trig Pin
int Fspeedd = 0; // Front Spped
int Rspeedd = 0; // Right Spped
int Lspeedd = 0; // Left Spped
int directionn = 0; // F=8 B=2 L=4 R=6
Servo myservo; // myservo object
int delay_time = 250; // Servo stable time
int Fgo = 8; // F
int Rgo = 6; // R
int Lgo = 4; // L
int Bgo = 2; // B
void setup()
{
//This is the beginning of the code
Serial.begin(9600); // Set Serial port rate
pinMode(pinLB,OUTPUT); // set PWM pin mode
pinMode(pinLF,OUTPUT); // set PWM pin mode
pinMode(pinRB,OUTPUT); // set PWM pin mode
pinMode(pinRF,OUTPUT); // set PWM pin mode
pinMode(motorA,OUTPUT); // set PWM pin mode
pinMode(motorB,OUTPUT); // set PWM pin mode
pinMode(inputPin, INPUT); // set Ultrasonic Sensor Echo PWM pin mode
pinMode(outputPin, OUTPUT); // set Ultrasonic Sensor Trig PWM pin mode
myservo.attach(5); // Use PWM Pin 5 as the Servo control pin
}
void advance(int a) // Go Ahead
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
digitalWrite(pinRB,LOW);
analogWrite(pinRF,153);
digitalWrite(pinLB,LOW);
analogWrite(pinLF,153);
delay(a * 100);
}
void right(int b) //Trun Right(Single Wheel)
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
digitalWrite(pinRB,LOW); //Right Back Scroll
digitalWrite(pinRF,LOW);
digitalWrite(pinLB,HIGH);
digitalWrite(pinLF,LOW);
delay(b * 100);
}
void left(int c) //Trun Left(Single Wheel)
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
digitalWrite(pinRB,LOW);
digitalWrite(pinRF,HIGH);
digitalWrite(pinLB,LOW); //Left Back Scroll
digitalWrite(pinLF,LOW);
delay(c * 100);
}
void turnR(int d) //Trun Right(Double Wheels)
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
digitalWrite(pinRB,HIGH); //Right Back Scroll
digitalWrite(pinRF,LOW);
digitalWrite(pinLB,LOW);
digitalWrite(pinLF,HIGH); //Left Front Scroll
delay(d * 100);
}
void turnL(int e) //Trun Left(Double Wheels)
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
digitalWrite(pinRB,LOW);
digitalWrite(pinRF,HIGH); //Right Front Scroll
digitalWrite(pinLB,HIGH); //Left Back Scroll
digitalWrite(pinLF,LOW);
delay(e * 100);
}
void stopp(int f) //Stop
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
digitalWrite(pinRB,HIGH);
digitalWrite(pinRF,HIGH);
digitalWrite(pinLB,HIGH);
digitalWrite(pinLF,HIGH);
delay(f * 100);
}
void back(int g) //Back
{
digitalWrite (motorA, HIGH);
digitalWrite (motorB, HIGH);
analogWrite (pinRB,255); //Right Back Scroll
digitalWrite(pinRF,LOW);
analogWrite(pinLB,255); //Left Back Scroll
digitalWrite(pinLF,LOW);
delay(g * 100);
}
void detection() //Detect 3 angles (0-left 90-ahead 180-right)
{
int delay_time = 250; // set servo stable time
ask_pin_F(); // read front length
if(Fspeedd < 20) // If length less than 25cm
{
stopp(10); // Stop
ask_pin_L(); // Check the left length
delay(delay_time); // Wait Servo stable
ask_pin_R(); // Check the right length
delay(delay_time); // Wait Servo stable
if(Lspeedd > Rspeedd) //If left length greater than right length
{
directionn = Lgo; //Then trun left
}
if(Lspeedd <= Rspeedd) //If left length equal or less than right length
{
directionn = Rgo; //Then trun right
}
if (Lspeedd < 20 && Rspeedd < 20) //if left length and right length both less than 10cm
{
directionn = Bgo; //Then go back
}
}
else //Other condition
{
directionn = Fgo; //Go ahead
}
}
void ask_pin_F() // Read front length
{
myservo.write(90); // Point to the 90 degree(ahead)
digitalWrite(outputPin, LOW); // Keep ultrasonic send LOW PWM for 2μs
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // Keep ultrasonic send HIGH PWM for at least 10μs
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // Keep Low output
float Fdistance = pulseIn(inputPin, HIGH); // Read the time difference
Fdistance= Fdistance/29/2; // Calculate length (cm)
Serial.print("F distance:"); // Print the length the Serial Monitor
Serial.println(Fdistance); //
Fspeedd = Fdistance; // Save spped to the variable
}
void ask_pin_L() // Read left length
{
myservo.write(0);
delay(delay_time);
digitalWrite(outputPin, LOW);
delayMicroseconds(2);
digitalWrite(outputPin, HIGH);
delayMicroseconds(10);
digitalWrite(outputPin, LOW);
float Ldistance = pulseIn(inputPin, HIGH);
Ldistance= Ldistance/29/2;
Serial.print("L distance:");
Serial.println(Ldistance);
Lspeedd = Ldistance;
}
void ask_pin_R() // 量出右邊距離
{
myservo.write(180);
delay(delay_time);
digitalWrite(outputPin, LOW);
delayMicroseconds(2);
digitalWrite(outputPin, HIGH);
delayMicroseconds(10);
digitalWrite(outputPin, LOW);
float Rdistance = pulseIn(inputPin, HIGH);
Rdistance= Rdistance/29/2;
Serial.print("R distance:");
Serial.println(Rdistance);
Rspeedd = Rdistance;
}
void loop()
{
myservo.write(90); // Point to the front
detection(); // Check the distance
if(directionn == 2) // Go Back
{
back(10);
Serial.print(" Reverse ");
}
if(directionn == 4) // Turn Right
{
turnR(3);
Serial.print(" Right ");
}
if(directionn == 6) // Turn Left
{
turnL(3);
Serial.print(" Left ");
}
if(directionn == 8) // Go ahead
{
advance(8);
Serial.print(" Advance ");
Serial.print(" ");
}
}