Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
This sample code is for Obstacles Avoiding Robot with Maker Drive.
#include "CytronMotorDriver.h"
// Configure the motor driver.
CytronMD motor1(PWM_PWM, 3, 9); // PWM 1A = Pin 3, PWM 1B = Pin 9.
CytronMD motor2(PWM_PWM, 10, 11); // PWM 2A = Pin 10, PWM 2B = Pin 11.
long duration;
long distance;
#define BUTTON 2
#define PIEZO 8
#define TRIGPIN 5
#define ECHOPIN 4
#define NOTE_G4 392
#define NOTE_C5 523
#define NOTE_G5 784
#define NOTE_C6 1047
int startMelody[] = {NOTE_G5, NOTE_C6};
int startNoteDurations[] = {12, 8};
int stopMelody[] = {NOTE_C6, NOTE_G5};
int stopNoteDurations[] = {12, 8};
#define playStartMelody() playMelody(startMelody, startNoteDurations, 2)
#define playStopMelody() playMelody(stopMelody, stopNoteDurations, 2)
char inChar;
String inString;
void setup()
{
pinMode(BUTTON, INPUT_PULLUP);
pinMode(PIEZO, OUTPUT);
pinMode(TRIGPIN, OUTPUT);
pinMode(ECHOPIN, INPUT);
Serial.begin(9600);
}
void loop()
{
if (digitalRead(BUTTON) == LOW) {
playStartMelody();
while (true) {
digitalWrite(TRIGPIN, LOW); // Clears the TRIGPIN
delayMicroseconds(2);
digitalWrite(TRIGPIN, HIGH); // Sets the TRIGPIN on HIGH state for 10 micro seconds
delayMicroseconds(10);
digitalWrite(TRIGPIN, LOW);
duration = pulseIn(ECHOPIN, HIGH); // Reads the ECHOPIN, returns the sound wave travel time in microseconds
distance = duration * 0.034 / 2; // Calculating the distance
if (distance < 10) { // If the obstacles is less then 10 cm in front of the robot
robotTurnRight();
Serial.print("Distance: ");
Serial.println(distance);
}
else if (digitalRead(BUTTON) == LOW) {
robotStop();
playStopMelody();
break;
}
else { // If no obstacles in front of the robot
robotForward();
Serial.print("Distance: ");
Serial.println(distance);
}
}
}
}
void playMelody(int *melody, int *noteDurations, int notesLength)
{
pinMode(PIEZO, OUTPUT);
for (int thisNote = 0; thisNote < notesLength; thisNote++) {
int noteDuration = 1000 / noteDurations[thisNote];
tone(PIEZO, melody[thisNote], noteDuration);
int pauseBetweenNotes = noteDuration * 1.30;
delay(pauseBetweenNotes);
noTone(PIEZO);
}
}
void robotStop()
{
motor1.setSpeed(0); // Motor 1 stops.
motor2.setSpeed(0); // Motor 2 stops.
}
void robotForward()
{
motor1.setSpeed(200); // Motor 1 runs forward.
motor2.setSpeed(200); // Motor 2 runs forward.
}
void robotReverse()
{
motor1.setSpeed(-200); // Motor 1 runs backward.
motor2.setSpeed(-200); // Motor 2 runs backward.
}
void robotTurnLeft()
{
motor1.setSpeed(-200); // Motor 1 runs forward.
motor2.setSpeed(200); // Motor 2 runs backward.
}
void robotTurnRight()
{
motor1.setSpeed(200); // Motor 1 runs backward.
motor2.setSpeed(-200); // Motor 2 runs forkward.
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.