Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
LegoServoCar
// ---------------------------------------------------------------------------
// Sketch for a Lego car with two servos (with continuous rotation) for left
// and right wheel and an SR04 ultra sonic sensor for measuring the distance to
// objects in front of the car
// ---------------------------------------------------------------------------
#include <NewPing.h>
#include <Servo.h>
//
// SR04 distance sensor setup
//
#define TRIGGER_PIN 6 // Arduino pin for trigger on sensor
#define ECHO_PIN 7 // Arduino pin for echo on sensor
#define MAX_DISTANCE 100 // Maximum distance (in centimeters) to detect
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
unsigned long distance = 0;
//
// On/off button setup
//
#define BUTTON_PIN 8
bool runCar = false;
//
// Servo setup
//
#define RIGHT_WHEEL_PIN 10
#define LEFT_WHEEL_PIN 9
#define RIGHT_WHEEL_STOP_POSITION 88
#define LEFT_WHEEL_STOP_POSITION 89
Servo rightWheel;
Servo leftWheel;
void setup()
{
pinMode(BUTTON_PIN,OUTPUT);
rightWheel.attach(RIGHT_WHEEL_PIN);
leftWheel.attach(LEFT_WHEEL_PIN);
Stop();
Serial.begin(115200);
}
void loop()
{
CheckButton();
if (runCar)
{
MeasureDistance();
OperateCar();
}
delay(200);
}
void OperateCar()
{
if (distance > 0 && distance < 50)
{
BackwardsWithLeftTurn();
}
else
{
Forward();
}
}
void CheckButton()
{
int buttonState = digitalRead(BUTTON_PIN);
if (buttonState == HIGH)
{
if (!runCar)
{
runCar = true;
}
else
{
runCar = false;
Stop();
}
delay(500);
}
}
void MeasureDistance()
{
int uS = sonar.ping_median(10);
distance = uS / US_ROUNDTRIP_CM;
Serial.println(distance);
}
void Forward()
{
Serial.println("Forward");
rightWheel.write(RIGHT_WHEEL_STOP_POSITION-10);
leftWheel.write(LEFT_WHEEL_STOP_POSITION+10);
}
void BackwardsWithLeftTurn()
{
Serial.println("Backwards with turn");
rightWheel.write(RIGHT_WHEEL_STOP_POSITION+10);
leftWheel.write(LEFT_WHEEL_STOP_POSITION+10);
}
void Stop()
{
Serial.println("Stop");
rightWheel.write(RIGHT_WHEEL_STOP_POSITION);
leftWheel.write(LEFT_WHEEL_STOP_POSITION);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment