Hi all! So i cam across a project online called buddy. it's essentially a handful of servos and an ultrasonic sensor that looks around the room and reacts. so i downloaded the code, and bought the parts, but it's not working as expected. The sensor doesn't actually seem to be reading the distance, and i'm quite the beginner at code and don't really know where i need to be looking.
//#include <mapping.h>
//started work on 9/19
// 1_0 worked well with botclock2_2
#include <Servo.h> //arduino library
#include <math.h> //standard c library
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement
Servo baseServo;
Servo nodServo;
Servo tiltServo;
struct headPos {
int baseServoAngle;
int nodServoAngle ;
int tiltServoAngle ;
int desiredDelay ;
};
struct headPos faceMotion;
#define echoPin A2 // Echo Pin
#define trigPin A3 // Trigger Pin
//int desiredDelay = 16;
int ready = 0;
int randomNumber = 0;
// Define the default startup mode
int robotMode = 700;
int buzzerTone = 500;
//+++++++++++++++ULTRASONIC VARIABLES++++++++++++++++++++++++++++
#define echoPin A2 // Echo Pin
#define trigPin A3 // Trigger Pin
#define buzzerPin 10 // Pin for the buzzer
bool holder = 1;
int maximumRange = 200; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long readDistance; // the output distance from the sensor
//+++++++++++++++FUNCTION DECLARATIONS+++++++++++++++++++++++++++
int ultraSensor(int theEchoPin, int theTrigPin);
void moveTo( struct headPos faceMotion);
void Speak3 (int soundPin, int currentTone, int finalTone);
void storedAction(int positionSelected, int theSpeed);
void speakWalter (int soundPin, int maxWords);
int servoParallelControl (int thePos, Servo theServo, int theSpeed );
void generateAction();
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void setup()
{
Serial.begin(9600);
baseServo.attach(2);
nodServo.attach(3);
tiltServo.attach(4);
Serial.setTimeout(50); //ensures the the arduino does not read serial for too long
Serial.println("started");
baseServo.write(90); //intial positions of servos
nodServo.write(90);
tiltServo.write(90);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(buzzerPin, OUTPUT);
ready = 0;
noTone(buzzerPin);
Serial.println("Ultrasonic Sensor HC-SR04 Test"); // print some text in Serial Monitor
Serial.println("with Arduino UNO R3");
}
void loop()
{
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
// read a usb command if available
if (Serial.available()) {
// read what type of a command will be sent
robotMode = Serial.parseInt();
if (robotMode == 200) {
faceMotion.baseServoAngle = Serial.parseInt();
faceMotion.nodServoAngle = Serial.parseInt();
faceMotion.tiltServoAngle = Serial.parseInt();
//buzzerTone = Serial.parseInt();
if (Serial.read() == '\n') { // if the last byte is 'd' then stop reading and execute command 'd' stands for 'done'
Serial.flush(); //clear all other commands piled in the buffer
Serial.print('d'); //send completion of the command
}
}
}
//++++++++++++++++Decision of process to use+++++++++++++++++++++++++++
if (robotMode == 500) {
//Go Compeletely Silent
Serial.print('d'); //send completion of the command
Serial.flush(); //clear all other commands piled in the buffer
}
if (robotMode == 600) {
// Alarm Sequence
Serial.print('d'); //send completion of the command
Serial.flush(); //clear all other commands piled in the buffer
tone(buzzerPin, 1000);
//delay(5000);
noTone(buzzerPin);
}
if (robotMode == 700) {
//Normal Interaction Mode
Serial.print('d'); //send completion of the command
Serial.flush(); //clear all other commands piled in the buffer
//read the distance read by the sensor
readDistance = 100;//ultraSensor(echoPin, trigPin);
if (readDistance > 80) {
int nothingCount = 0;
generateAction();
//Check an area in the map
speakWalter(buzzerPin, random(1, 25));
//this is where all the fun starts
randomNumber = random(1, 10); // find a random whole number between the two values
int randomIterations = random (1,5);
//Serial.println(randomNumber);
//run through s set of random actions
int i;
for (i=1; i<randomIterations; i++){
generateAction();
}
//storedAction(randomNumber, 7);
}
else if (readDistance <= 6) {
// do something when this close
//fast response in surprise
}
else {
// occassionally check map and inspect world in general.
}
} // end of 700 if mode
// ---------------------------------------Act Upon Mode Type---------------------------------------------
// //++++++++++++++++++Remote Mode++++++++++++++++++++++
// if (robotMode == 200) {
// //faceMotion.base
// tone(buzzerPin, buzzerTone);
// moveTo(faceMotion );
// //servoParallelControl ( baseServoAngle, baseServo, 5 );
// }
// // ++++++++++++++++Speech Mode+++++++++++++++++++++
// if (robotMode == 600) {
// //speakWalter(buzzerPin, 50);
// }
//
// // ++++++++++++++++Stopped Mode+++++++++++++++++++++
// if (robotMode == 700) {
//
// }
} // end of primary loop
//++++++++++++++++++++++++++++++FUNCTION DEFINITIONS++++++++++++++++++++++++++++++++++++++++++
int ultraSensor(int theEchoPin, int theTrigPin) {
//this fucntion caluclates and returns the distance in cm
long duration, distance; // Duration used to calculate distance
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
digitalWrite(theTrigPin, LOW);
delayMicroseconds(2);
digitalWrite(theTrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(theTrigPin, LOW);
duration = pulseIn(theEchoPin, HIGH);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration / 58.2;
return distance;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
void speakWalter (int soundPin, int maxWords) {
int toneDuration;
int numberOfWords;
int toneFreq; // frequency of tone created
int phraseDelay; // the time between individual statements
numberOfWords = random (1, maxWords);
//Serial.print("Number of words = ");
//Serial.println(numberOfWords);
// generate the random set of words
for ( int i; i <= numberOfWords; i++) {
toneDuration = random (25, 300);
toneFreq = random (200, 400);
tone(soundPin, toneFreq);
delay(toneDuration);
noTone(soundPin);
}
//phraseDelay = random(100, 10000);
//delay(phraseDelay);
//noTone(soundPin);
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Speak2 (int soundPin, int maxWords, Servo Rot1, Servo Nod1, Servo Tilt1) {
//function that links servo motion to sound
int toneDuration;
int numberOfWords;
int toneFreq;
numberOfWords = random (1, maxWords);
//Serial.print("Number of words = ");
//Serial.println(numberOfWords);
for ( int i; i <= numberOfWords; i++) {
// randomly generate the tone durations and freq
toneDuration = random (25, 150);
toneFreq = random (100, 1800);
// use the tone durations to define servo movemnt
//large number of tones
tone(soundPin, toneFreq);
delay(toneDuration);
noTone(soundPin);
}
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Speak3 (int soundPin, int currentTone, int finalTone) {
// has two notes meld into each other as a singer might
int toneDuration = 8;
//int numberOfWords;
//int toneFreq; // frequency of tone created
//int phraseDelay; // the time between individual statements
//numberOfWords = random (1,maxWords);
//Serial.print("Number of words = ");
//Serial.println(numberOfWords);
int theDiff = (finalTone - currentTone) / 5; //The difference between the values
if (theDiff > 0) {
// if ascending
for ( int i; i <= theDiff; i++) {
tone(soundPin, currentTone);
delay(toneDuration);
currentTone = currentTone + 5;
noTone(soundPin);
}
noTone(soundPin);
}
else {
theDiff = abs(theDiff);
for ( int i; i <= theDiff; i++) {
tone(soundPin, currentTone);
delay(toneDuration);
currentTone = currentTone - 5;
noTone(soundPin);
}
noTone(soundPin);
}
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void moveTo( struct headPos faceMotion) {
int status1 = 0;
int status2 = 0;
int status3 = 0;
int done = 0 ;
while ( done == 0) {
//move the servo to the desired position
//this loop will cycle through the servos sending each the desired position.
//Each call will cause the servo to iterate about 1-5 degrees
//the rapid cycle of the loop makes the servos appear to move simultaneously
status1 = servoParallelControl(faceMotion.baseServoAngle, baseServo, faceMotion.desiredDelay);
status2 = servoParallelControl(faceMotion.nodServoAngle, nodServo, faceMotion.desiredDelay);
status3 = servoParallelControl(faceMotion.tiltServoAngle, tiltServo, faceMotion.desiredDelay);
//continue until all have reached the desired position
if (status1 == 1 & status2 == 1 & status3 == 1 ) {
done = 1;
}
}// end of while
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int servoParallelControl (int thePos, Servo theServo, int theSpeed ) {
int startPos = theServo.read(); //read the current pos
int newPos = startPos;
//int theSpeed = speed;
//define where the pos is with respect to the command
// if the current position is less that the actual move up
if (startPos < (thePos - 5)) {
newPos = newPos + 1;
theServo.write(newPos);
delay(theSpeed);
return 0;
}
else if (newPos > (thePos + 5)) {
newPos = newPos - 1;
theServo.write(newPos);
delay(theSpeed);
return 0;
}
else {
return 1;
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void generateAction() {
int waypoint = 0;
int theJoint = 0; // one of the joints in a trajectory array
int trajSize = 0;
int theSpeed = 5;
struct headPos newPosition;
newPosition.desiredDelay = theSpeed;
delay (random (100, 1000)); //delay between ne motions
theSpeed = random (1, 7);
newPosition.tiltServoAngle = random (20, 150); //actually nod)
newPosition.baseServoAngle = random (10, 170);
newPosition.nodServoAngle = random (80, 150); //50 min j(bottom, top)
newPosition.desiredDelay = theSpeed;
moveTo (newPosition);
}
//++++++++++++++++++++++++++++
void checkMap() {
//randomly cycle and move to positions to check if there is any item at that location. (create behaviors for moving to those locations without the
}
//+++++++++++++++++++++++++++
void storedAction(int positionSelected, int theSpeed) {
int waypoint = 0;
int theJoint = 0; // one of the joints in a trajectory array
int trajSize = 0;
struct headPos newPosition;
newPosition.desiredDelay = theSpeed;
if (positionSelected == 1) {
// up
int trajSize = 1;
int trajectory[trajSize][5] = {{101, 65, 153, 75, 5} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
else if (positionSelected == 2) {
//shake
int trajSize = 9;
int trajectory[trajSize][5] = {{97, 65, 130, 75, 5}, {97, 101, 130, 75, 5}, {97, 71, 130, 75, 5}, {97, 114, 130, 75, 5}, {97, 70, 130, 75, 5}, {97, 107, 130, 75, 5}, {101, 79, 146, 75, 8}, {101, 56, 146, 75, 8}, {101, 81, 146, 75, 8} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
else if (positionSelected == 6) {
//nod head
int trajSize = 9;
int trajectory[trajSize][5] = {{97, 70, 130, 75, 5}, {97, 70, 117, 75, 5} , {97, 70, 141, 75, 5}, {97, 70, 112, 75, 5}, {97, 70, 143, 75, 5}, {97, 70, 115, 75, 5}, {97, 70, 146, 75, 5}, {97, 70, 115, 75, 5}, {97, 70, 144, 75, 5} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
else if (positionSelected == 7) {
//hang and shake
int trajSize = 5;
int trajectory[trajSize][5] = {{101, 65, 111, 75, 12}, {101, 99, 111, 75, 8} , {101, 43, 111, 75, 8}, {101, 101, 111, 75, 8}, {101, 48, 111, 75, 8} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
else if (positionSelected == 9) {
//excited
int trajSize = 11;
int trajectory[trajSize][5] = {{89, 76, 143, 5, 5}, {114, 76, 143, 5, 5} , {87, 76, 143, 5, 5}, {114, 76, 143, 5, 5}, {88, 76, 143, 5, 5}, {121, 76, 143, 5, 5}, {91, 76, 143, 5, 5}, {115, 76, 143, 5, 5}, {88, 76, 143, 5, 5}, {117, 76, 143, 5, 5}, {96, 76, 143, 5, 5} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
else if (positionSelected == 10) {
//Jump
int trajSize = 3;
int trajectory[trajSize][5] = {{97, 70, 160, 75, 10}, {97, 70, 65, 75, 1} , {97, 70, 130, 75, 5} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
newPosition.desiredDelay = trajectory[waypoint][theJoint + 4];
moveTo (newPosition);
waypoint++;
}
}
else {
//nod head
int trajSize = 9;
int trajectory[trajSize][5] = {{97, 70, 130, 75, 5}, {97, 70, 117, 75, 5} , {97, 70, 141, 75, 5}, {97, 70, 112, 75, 5}, {97, 70, 143, 75, 5}, {97, 70, 115, 75, 5}, {97, 70, 146, 75, 5}, {97, 70, 115, 75, 5}, {97, 70, 144, 75, 5} };
while (waypoint < trajSize) {
newPosition.tiltServoAngle = trajectory[waypoint][theJoint];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 1];
newPosition.nodServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
}Here's the original code. ^
I've tried running this sketch...
// ---------------------------------------------------------------- //
// Arduino Ultrasoninc Sensor HC-SR04
// Re-writed by Arbi Abdul Jabbaar
// Using Arduino IDE 1.8.7
// Using HC-SR04 Module
// Tested on 17 September 2019
// ---------------------------------------------------------------- //
#define echoPin A2 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin A3 //attach pin D3 Arduino to pin Trig of HC-SR04
// defines variables
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement
void setup() {
pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT
Serial.begin(9600); // // Serial Communication is starting with 9600 of baudrate speed
Serial.println("Ultrasonic Sensor HC-SR04 Test"); // print some text in Serial Monitor
Serial.println("with Arduino UNO R3");
}
void loop() {
// Clears the trigPin condition
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin HIGH (ACTIVE) for 10 microseconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
// Displays the distance on the Serial Monitor
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
}and this one works fine, so i know my sensor is ok. i've then tried adapting the second code to work with the original code so i could get a readout, and its just reading at 0cm constantly.
any suggestions would be amazing!
Thanks in advance.
but I think his efforts to point out the line to correct should not be in vain ...