Hi everyone. i've a project i'm working on with servos, an ultrasonic sensor and some other bits an pieces. there's a part of the code where if something comes up close to the sensor, the servos react to it. i'm wanting to add a choice between three different actions here. i'm under the impression i can do this using 'case' but i've never used it before, and i'm a bit stumped on how to implement it.
I've had a google and can't really find how to fit existing projects to my needs.
Any pointers would be great!
The part i'm wanting to add to is labelled //THIS IS WHERE I WANT THE CHOICE!!
and is about 1/3 of the way down the code.
#include "SoftwareSerial.h"
#include "DFRobotDFPlayerMini.h"
// Use pins 2 and 3 to communicate with DFPlayer Mini
static const uint8_t PIN_MP3_TX = 9; // Connects to module's RX
static const uint8_t PIN_MP3_RX = 10; // Connects to module's TX
SoftwareSerial softwareSerial(PIN_MP3_RX, PIN_MP3_TX);
// Create the Player object
DFRobotDFPlayerMini player;
#include <Servo.h> //arduino library
#include <math.h> //standard c librarz
int Reset = 3;
Servo baseServo;
Servo arielServo;
Servo tiltServo;
struct headPos {
int baseServoAngle;
int arielServoAngle ;
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()
{
digitalWrite(Reset, HIGH);
delay(200);
pinMode(Reset, OUTPUT);
Serial.begin(9600);
softwareSerial.begin(9600);
Serial.println("Beginning bootup protocols.");
delay(200);
digitalWrite(Reset, LOW);
delay(200);
if (player.begin(softwareSerial)) {
Serial.println("Vocal Module Engaging...");
player.volume(20); ////////////////////////////////////////////////////////////////////////////////////////////////////VOLUME!!!!!
baseServo.attach(2);
arielServo.attach(4);
tiltServo.attach(3);
baseServo.write(90); //intial positions of servos
arielServo.write(90);
tiltServo.write(90);
delay (3000);
player.playMp3Folder(0001);
delay (4000);
Serial.println("Vocal Module Engaged");
} else {
Serial.println("Vocal Module Failed to Engage");
}
Serial.setTimeout(50); //ensures the the arduino does not read serial for too long
Serial.println("Cognitive functionality operating as expected.");
baseServo.write(90); //intial positions of servos
arielServo.write(90);
tiltServo.write(90);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(buzzerPin, OUTPUT);
ready = 0;
noTone(buzzerPin);
}
void loop()
{
// 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.arielServoAngle = 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.println('d'); //send completion of the command
}
}
}
//++++++++++++++++Decision of process to use+++++++++++++++++++++++++++
if (robotMode == 500) {
//Go Compeletely Silent
Serial.println('d'); //send completion of the command
Serial.flush(); //clear all other commands piled in the buffer
}
if (robotMode == 600) {
// Alarm Sequence
Serial.println('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.println("Observing..."); //send completion of the command
Serial.flush(); //clear all other commands piled in the buffer
//read the distance read by the sensor
readDistance = ultraSensor(echoPin, trigPin);
if (readDistance >= 30) {
int nothingCount = 0;
generateAction();
//Check an area in the map
//this is where all the fun starts
randomNumber = random(1, 6); // 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, 2);
}
else { /////////////////////////////////////////////////////////////////////////////////////////////////////THIS IS WHERE I WANT THE CHOICE!!
}
} // 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.arielServoAngle, arielServo, 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 = 0;
struct headPos newPosition;
newPosition.desiredDelay = theSpeed;
////////////////////////////////////////////////////////////////////////////////////////////////////HERE
player.volume(20); ////////////////////////////////////////////////////////////////////////////////////////////////////VOLUME!!!!!
delay; 100;
player.playMp3Folder(random(2, 98));
delay; 1000;
delay (random (1000, 2000)); //delay between ne motions
theSpeed = random (1, 10);
newPosition.tiltServoAngle = random (10, 120);
newPosition.baseServoAngle = random (10, 170);
newPosition.arielServoAngle = random (70, 130);
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.arielServoAngle = 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.arielServoAngle = 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.arielServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
else if (positionSelected == 7) {
//hang and shake
int trajSize = 55;
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 + 0];
newPosition.baseServoAngle = trajectory[waypoint][theJoint + 0];
newPosition.arielServoAngle = trajectory[waypoint][theJoint + 0];
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.arielServoAngle = 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.arielServoAngle = 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.arielServoAngle = trajectory[waypoint][theJoint + 2];
moveTo (newPosition);
waypoint++;
}
}
}
The code i'm wanting to implement is here:
Case 1.
Serial.println(“case 1.”);
player.playMp3Folder(random(99, 103));
baseServo.write(90);
arielServo.write(90);
delay(100);
arielServo.write(130);
delay(100);
arielServo.write(90);
delay(100);
arielServo.write(130);
delay(100);
arielServo.write(90);
delay(100);
arielServo.write(130);
delay(100);
arielServo.write(90);
delay(100);
arielServo.write(130);
delay(100);
arielServo.write(90);
delay(100);
arielServo.write(130);
delay(100);
arielServo.write(90);
delay(1000);
and case 2.
Serial.println(“Case 2.”);
player.playMp3Folder(random(99, 103));
baseServo.write(90);
arielServo.write(90);
delay(20);
arielServo.write(130);
delay(20);
arielServo.write(90);
delay(20);
arielServo.write(130);
delay(20);
arielServo.write(90);
delay(20);
arielServo.write(130);
delay(20);
arielServo.write(90);
delay(20);
arielServo.write(130);
delay(20);
arielServo.write(90);
delay(20);
arielServo.write(130);
delay(20);
arielServo.write(90);
delay(1000);
haven't quite figured out what i want for case 3 yet, but once i know how to implement it, i should be able to figure it out!
Thanks in advance!
