A whole ton of Arduino Code

#define header 5 //this is the current header
#define header2 6
//Declare Output Pins
int motorPin = 13;
int speakerOut = 12;
//Declare Input Pins
int tablePin = 3;
int sensorPinX = 2;
int sensorPinY = 1;
int sensorPinZ = 0;
//Values for sensosrs
int valX = 0;
int valY = 0;
int valZ = 0;
//values for gesture recognition
int testVal = 111;
//Array to capture data
int newMotion[100];
int swirl[100] = {
511,718,426,512,717,541,513,717,603,513,716,637,513,716,652,
512,716,661,511,717,667,511,717,669,512,717,671,513,717,672,
514,715,673,514,715,674,513,715,674,512,715,674,512,717,674,
513,716,674,514,716,674,514,715,675,514,715,675,514,715,675,
513,715,675,513,716,674,514,716,674,514,716,674,514,715,674,
515,714,674,515,714,674,514,713,674,513,714,674,513,714,673,
514,714,674,515,712,674,514,712,674,513};
int shake[100] = {
507,578,343,504,575,435,502,573,482,506,571,512,508,579,
528,514,585,538,519,585,548,524,595,555,527,591,557,528,
597,560,531,598,562,533,591,562,534,598,564,536,595,560,
539,594,560,542,596,561,542,590,560,542,593,561,542,591,
557,542,586,557,540,587,557,538,582,553,536,581,553,535,
580,551,534,574,547,534,573,546,531,571,543,530,569,540,
528,570,538,527,566,535,526,565,534,524,564,533,522,560,
532,520};
//Booleans to Control Behaviors
boolean swirlGate = false;
boolean shakeGate = false;
boolean chirpGate = true;
// Behavior Control
int loc = 0;
int behav = 0;
int shFactor = 0;
int swFactor = 0;
int randNum = 0;
int maxLoop = 0;
int getByte = -1;
int getByteL = -1;
int getByteB = -1;
int otherLoc = -1;
int otherBehav = -1;
//Xbee Config from Rob Faludi at ITP rob@faldui.com
// a byte to send out data:
char thisByte = 0;

void setup () {
pinMode(speakerOut, OUTPUT);
pinMode(tablePin, INPUT);
pinMode(sensorPinX, INPUT);
pinMode(sensorPinY, INPUT);
pinMode(sensorPinZ, INPUT);
//open serial
Serial.begin(9600);
//code from Rob Fauldi
// for some reason it seems to help to send an arbitrary character first
//then pause for the guard time before requesting command mode
Serial.print("X");
delay(1100);
// put the XBee in command mode
Serial.print("+++");
delay(1100);
// wait for a response from the XBee for 2000 ms, or start
// over with setup if no valid response comes


if (returnedOK() == 'T') {
// if an OK was received then continue
}
else {
setup(); // otherwise go back and try setup again
}
// set the PAN (personal area network) ID number
// this example uses 0x3330, but you'll want to choose your own
// unique hexadecimal number between 0x0 and 0xFFFE
// (note the comma at the end of the command which indicates that
//another command will follow)
//Serial.print("ATID3330,");
// set the Destination High to 0x0
// to select 16 bit addressing mode. These addresses can
// be assigned and changed by sending commands from a microcontroller
//Serial.print("DH0,");
// set the Destination Low (16 bit address)
// this example uses 0x0 for send and 0x1 for receive but you'll
// want to choose your own hexadecimal numbers between 0x0 and 0xFFFE
//Serial.print("DL1,");
// exit command mode (note that we use Serial.printLN here to issue
//a linefeed that completes the command sequence)
//Serial.println("CN");
// the preceeding commands can also be sent on a single line like this,
//using a single AT command with commas:
// Serial.println("ATID3330,DH0,DL1,CN");
// the preceeding command line could also be sent as separate commands,
//by reissuing the AT command:
//This part I modified for this project
Serial.println("ATID3330");
Serial.println("ATMY1");
Serial.println("ATDH0");
Serial.println("ATDLFFFF");
Serial.println("ATCN");
//end of modification
// wait for a response from the XBee for 2000 ms, or start
// over with setup if no valid response comes

if (returnedOK() == 'T') {
// if an OK was received then continue
}
else {
setup(); // otherwise go back and try setup again
}
}
//the returnOK function was written by Rob Faludi
char returnedOK () {
// this function checks the response on the serial port to see if it was an "OK" or not
char incomingChar[3];
char okString[] = "OK";
char result = 'n';
int startTime = millis();
while (millis() - startTime < 2000 && result == 'n') { // use a timeout of 10 seconds
if (Serial.available() > 1) {
// read three incoming bytes which should be "O", "K", and a linefeed:
for (int i=0; i<3; i++) {
incomingChar[i] = Serial.read();
}
if ( strstr(incomingChar, okString) != NULL ) { // check to see if the respose is "OK"
// if (incomingChar[0] == 'O' && incomingChar[1] == 'K') { // check to see if the first two characters are "OK"
result = 'T'; // return T if "OK" was the response
}
else {
result = 'F'; // otherwise return F
}
}
}
return result;
}

void recognizeSwirl(){
for(int z = 0; z < 100; z++){
//check new array to old
testVal = abs(newMotion[z] - swirl[z]);
//Serial.println(testVal);
if(testVal<65) {
swirlGate = true;
}
else {
swirlGate = false;
z = 100;
}
}
}

void recognizeShake(){
for(int z = 0; z < 100; z++){
//check new array to old
testVal = abs(newMotion[z] - shake[z]);
//Serial.println(testVal,DEC);
if(testVal<20) {
shakeGate = true;
}
else {
swirlGate = false;
z = 100;
}
}
}

void chirp(){
for(int x = 0; x< 5; x++){
for (int i = 0; i < 20; i++){
//This sets the frequence f = 1/t or PW = 1/(2*f) time is in microseconds
digitalWrite(speakerOut,HIGH);
delayMicroseconds(850);
digitalWrite(speakerOut,LOW);
delayMicroseconds(850);
}
delay(10);
}
delay(30);
for (int i = 0; i < 10; i++){
//This sets the frequence f = 1/t or PW = 1/(2*f) time is in microseconds
digitalWrite(speakerOut,HIGH);
delayMicroseconds(700);
digitalWrite(speakerOut,LOW);
delayMicroseconds(700);
}

}

void shakeLoop(){
//This for loop sets the length of the note
for(int x = 0; x< 3; x++){
for (int i = 0; i < 225; i++){
//This sets the frequence f = 1/t or PW = 1/(2*f) time is in microseconds
digitalWrite(speakerOut,HIGH);
delayMicroseconds(2000);
digitalWrite(speakerOut,LOW);
delayMicroseconds(2000);

}
delay(50);
}
delay(30);
for (int i = 0; i < 100; i++){
//This sets the frequence f = 1/t or PW = 1/(2*f) time is in microseconds
digitalWrite(speakerOut,HIGH);
delayMicroseconds(2500);
digitalWrite(speakerOut,LOW);
delayMicroseconds(2500);
}
delay(10);
for (int i = 0; i < 80; i++){
//This sets the frequence f = 1/t or PW = 1/(2*f) time is in microseconds
digitalWrite(speakerOut,HIGH);
delayMicroseconds(900);
digitalWrite(speakerOut,LOW);
delayMicroseconds(1500);
}
maxLoop++;
if (maxLoop >= 10){
behav = 0;
maxLoop = 0;
}
}

void swirlLoop(){
//Sound and Vibration
for(int x = 0; x< 2; x++){
for (int i = 0; i < 500; i++){
//This sets the frequence f = 1/t or PW = 1/(2*f) time is in microseconds
digitalWrite(speakerOut,HIGH);
delayMicroseconds(400+i);
digitalWrite(speakerOut,LOW);
delayMicroseconds(400+i);
}
delay(500);
}

maxLoop++;
if (maxLoop >= 10){
behav = 0;
maxLoop = 0;
}
}

int shakeFactor(int A, int B, int L){
int factor = 0;
int lfact = 0;
lfact = abs(loc - L);
factor = A + 20 - lfact;
return factor;
//calculate the shake control factor
}

int swirlFactor(int A,int B, int L){
int factor = 0;
int lfact = 0;
lfact = abs(loc - L);
factor = A + 40 - lfact;
return factor;
//calculate the shake control factor
}

void locate(){
int tableRead = 0;
tableRead = analogRead(tablePin);
if (tableRead>=128 && tableRead<=130){
loc = 1;
}
else if (tableRead>=1 && tableRead<=4){
loc = 2;
}
else if (tableRead>=66 && tableRead<=68){
loc = 3;
}
else if (tableRead>=60 && tableRead<=62){
loc = 4;
}
else if (tableRead==0){
loc = 0;
}
else {
locate();
}

}

void loop () {
locate();
Serial.print(header);
Serial.print(loc,DEC);
Serial.println(behav,DEC);
while(loc>0) { //Creature is in the table
chirpGate = true;
shFactor = 0; //sets controls back to zero
swFactor = 0; //sets controls back to zero
//sends out behaviors and location
locate();
//Receive Data
Serial.print(header);
Serial.print(loc,DEC);
Serial.println(behav,DEC);
if (Serial.available() > 1) {
getByte = Serial.read();
Serial.println(getByte,DEC);
if (getByte == 54){
getByteL = Serial.read();
getByteB = Serial.read();
if (getByteL == 49 ){
otherLoc = 1;
}
if (getByteL == 50){
otherLoc = 2;
}
if (getByteL == 51){
otherLoc = 3;
}
if (getByteL == 52){
otherLoc = 4;
}
if (getByteB == 48 ){
otherBehav = 0;
}
if (getByteB == 49){
otherBehav = 1;
}
if (getByteB == 50){
otherBehav = 2;
}
}
}
if (otherBehav==1){
swFactor = swirlFactor(swFactor,otherBehav,otherLoc); //these two functions will calculate a variable to control the switching of behaviors
}
else if (otherBehav==2){
shFactor = shakeFactor(shFactor,otherBehav,otherLoc); //these two functions will calculate a variable to control the switching of behaviors
}
if (behav == 0){ //If I am doing nothing
randNum = random(0,100);
if (randNum <= shFactor){ //should I be shaking
behav = 2;
maxLoop = 0;
shakeLoop();
}
else if (randNum <= swFactor){ //should I be swirling
behav = 1;
maxLoop = 0;
swirlLoop();
}
else{
chirp(); //Default Behavior
delay(5000);
}
}
if (behav == 2){ //If I am shaking
randNum = random(0,100);
if (randNum <= swFactor){
behav = 1;
maxLoop = 0;
swirlLoop();
}
else{
shakeLoop(); //Shake Behavior
delay(2000); //delay about four seconds
}
}

if (behav == 1){ //If I am swirling
randNum = random(0,100);
if (randNum <= shFactor){
behav = 2;
maxLoop = 0;
shakeLoop();
}
else{
swirlLoop(); //Shake Behavior
delay(4000); //delay about four seconds
}
}
}


//might not want a while statement here, if maybe??
while(loc == 0) { //This is when the creature is disconnected from the board
//Gesture Recognition goes here
locate();
if (chirpGate == true) {
chirp();
chirp();
chirpGate = false;
}
for(int i=0; i<100; i=i+3) {
newMotion[i] = analogRead(sensorPinX);
newMotion[i+1] = analogRead(sensorPinY);
newMotion[i+2] = analogRead(sensorPinZ);
}
recognizeSwirl();
if (swirlGate == false) {
recognizeShake();
}
if (swirlGate == true) {
//swirl Behavior
behav = 1;
swirlLoop();
// Serial.println("swirl");
}
if (shakeGate == true) {
//shake Behavior
behav = 2;
shakeGate = false;
shakeLoop();
//Serial.println("shake");
}
}
}