Hey guys,
Im building an object avoidance bot and I mounted two small speakers on it. I want it to play the Mario theme song as it navigates around. Having some trouble integrating the Mario song code to the avoidance code. Just cant get it to work properly. Can anybody help??? Ive been working on this project for a few months now. Almost done! Any help would be greatly appreciated.
Avoidance code:
#define servoPin 12 // Servomotor on pin D12.
#define pingpin 13 // Ping ultrasonic sensor on pin D13.
#define redled 9 // 2 drive motors, motor driver SN754410NE
#define whiteled 8
#define leftSpeedPin 10 // Left motor speed control
#define leftmotorpin1 5 // Left motor direction 1
#define leftmotorpin2 6 // Left motor direction 2
#define rightmotorpin1 3 // Right motor direction 1
#define rightmotorpin2 4 // Right motor direction 2
#define rightSpeedPin 11 // Right motor speed control
#define SERVOCENTER 1500 // Tweak so sensor looks straight ahead.
#define SERVORIGHT 550 // Servo position when looking right.
#define SERVOLEFT 2500 // Servo position when looking left.
int numPulses; // Number of pulses to servomotor.
void setup() {
pinMode(leftmotorpin1, OUTPUT); // Set motor pins as outputs.
pinMode(leftmotorpin2, OUTPUT);
pinMode(leftSpeedPin, OUTPUT);
pinMode(rightmotorpin1, OUTPUT);
pinMode(rightmotorpin2, OUTPUT);
pinMode(rightSpeedPin, OUTPUT);
pinMode(servoPin, OUTPUT); // Servomotor as output.
pinMode(redled, OUTPUT); // leds as outputs
pinMode(whiteled, OUTPUT);
centerServo(); // Centers Servo
delay(2000); // Wait 2 seconds
Serial.begin(115200);
}
void loop() {
int leftDistance, rightDistance, currDist; // Distance readings from Ping sensor.
int dangerlevel = 30; // How far away should things be, before we react?
// int numPulses;
currDist = readDistance();
if(currDist > dangerlevel) {
nodanger(); // If no danger, drive ahead
}
else {
whichway(); // If obstacle ahead then decide which way is better
}
}
void nodanger() { // Drive forward
digitalWrite(whiteled, HIGH);
digitalWrite(redled, LOW);
analogWrite(leftSpeedPin, 127); //PWM Speed Control 0 - 255
analogWrite(rightSpeedPin, 127);
digitalWrite(leftmotorpin1, HIGH);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, HIGH);
digitalWrite(rightmotorpin2, LOW);
return;
}
void whichway() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
totalhalt(); // First stop before looking left and right
int leftDistance, rightDistance, currDist;
for (numPulses = 0; numPulses < 20; numPulses++) // Turn servo left (look left)
{
pulseOut(servoPin, SERVOLEFT);
delay(40);
}
leftDistance = readDistance(); // Check distance
totalhalt(); // totalhalt also centers the servo
for (numPulses = 0; numPulses < 20; numPulses++) // Turn servo right (look right)
{
pulseOut(servoPin, SERVORIGHT);
delay(40);
}
rightDistance = readDistance(); // Check distance
totalhalt(); // totalhalt also centers the servo
if(leftDistance > rightDistance) { // Choose better direction. Greater space
body_leftturn();
}
else {
body_rightturn();
}
return;
}
void totalhalt() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
digitalWrite(leftmotorpin1, LOW);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, LOW);
digitalWrite(rightmotorpin2, LOW);
digitalWrite(leftSpeedPin, LOW);
digitalWrite(rightSpeedPin, LOW);
centerServo();
delay(1000);
}
void body_leftturn() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
digitalWrite(leftmotorpin1, LOW);
digitalWrite(leftmotorpin2, HIGH);
digitalWrite(rightmotorpin1, HIGH);
digitalWrite(rightmotorpin2, LOW);
digitalWrite(leftSpeedPin, HIGH);
digitalWrite(rightSpeedPin, HIGH);
delay(300);
totalhalt();
}
void body_rightturn() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
digitalWrite(leftmotorpin1, HIGH);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, LOW);
digitalWrite(rightmotorpin2, HIGH);
digitalWrite(leftSpeedPin, HIGH);
digitalWrite(rightSpeedPin, HIGH);
delay(300);
totalhalt();
}
void pulseOut(byte pinNumber, int duration)
{
digitalWrite(servoPin, HIGH);
delayMicroseconds(duration);
digitalWrite(servoPin, LOW);
}
void centerServo()
{
byte index;
for (index = 0; index < 20; index++)
{
pulseOut(servoPin, SERVOCENTER);
delay(20);
}}
long readDistance() {
long duration, inches, cm;
// The Ping is triggered by a HIGH pulse of 2 or more microseconds.
// We give a short LOW pulse beforehand to ensure a clean HIGH pulse.
pinMode(pingpin, OUTPUT);
digitalWrite(pingpin, LOW);
delayMicroseconds(2);
digitalWrite(pingpin, HIGH);
delayMicroseconds(5);
digitalWrite(pingpin, LOW);
// The same pin is used to read the signal from the Ping: a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingpin, INPUT);
duration = pulseIn(pingpin, HIGH);
// Convert the time into a distance.
cm = microsecondsToCentimeters(duration);
return(cm);
}
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
return microseconds / 29 / 2;}