Rudforce Intragalactic SVN 812r3

// 812-R3
//
// Another junkbot from Rudolph
// See more at http://letsmakerobots.com/node/8441

// 28 Jun '09
// eighthSpinTime is wrong
// still working on getting properly aimed at bright location
// need to get rid of all the Serial.print lines while looking for light

// 12 July '09
// rewrote part of light seeking routine, it works better now
// still need to clobber all Serial.print lines in light seeking part
// Set up a SVN repo to keep track of stuff! Yay for overkill!!!

#include <Servo.h>

// set this to 1 to Serial.print() when stuff happens
// no reason to use it when bot isn't tethered to PC
boolean _DEBUG = 0;

// Which pin is the speaker attached to?
int speakerPin = 8;

// which digital pins are the feelers attached to?
int frontFeeler = 2;
int rearFeeler = 3;

// this is an analog pin with nothing plugged into it.
// used for randomSeed() in setup()
int unusedAnalogPin = 0;

// the analog pin the LDR is plugged into
int eyePin = 1;

// which pins are the servos plugged into?
int leftServoPin = 9;
int rightServoPin = 10;

// centered/stop position of each servo. amazing they're the same!
int leftServoStop = 92;
int rightServoStop = 92;

// how long to drive each motor. they're different, stupd cheap servos...
// [][0] is left wheel, [][1] is right wheel, drive duration in millis
// [][2] is drive speed (in servo angle)
int servoDriveTimes[3][3] = {{1700, 1850, 112},    // UNTESTED!!! May be wrong durations
                             {1750, 1900, 110},
                             {2450, 2575, 100}
                            };

// how long it takes to spin in place 1/8 turn at speed spinSpeed
int eighthSpinTime = 300;
int spinSpeed = 102;

// the noise arrays, and the number of elements in each array
// blahArrayCount is needed to play random()-ed noises
int noiseArrayCount = 6;
int noiseArray[6][5] ={{2, 2200, 2100, 1800, 10}, // Default Sound
                       {75, 317, 745, 317, 43},   // Default 2
{1, 903, 835, 869, 100}, // click
{8, 1827, 1478, 1162, 46}, // question
{8, 542, 215, 610, 20}, // surprised
{32, 451, 452, 215, 27}, // question
                      };

int happyArrayCount = 9;
int happyArray[9][5] = {{50,400,200,400,50},  // yay!
{61, 339, 666, 542, 85}, // happy beedle
{51, 971, 486, 497, 100}, // happy
{73, 587, 486, 497, 37}, // happy
{20, 1478, 1, 1501, 100}, // happy
{16, 2211, 1501, 1951, 100}, // happy?
{52, 542, 215, 610, 49}, // cool, saw something
{1, 2211, 1478, 1951, 58}, // happy question?
{50, 903, 835, 869, 100}, // happy question beep
                      };

int sadArrayCount = 8;
int sadArray[8][5] = {{1,1500,2000,2000,10},  // "oh" sad
{8, 1827, 1478, 1162, 100}, // mad
{8, 2436, 2549, 2425, 100}, // sad
{18, 2617, 2842, 3000, 100}, // sad
{3, 2121, 2526, 3000, 100}, // sad
{26, 903, 835, 869, 10}, // sad
{26, 903, 948, 869, 10}, // sadder
{1, 2211, 2526, 1951, 30}, // sad
                    };

int angryArrayCount = 10;
int angryArray[10][5] = {{10, 2594, 2741, 2865, 46}, // mad
{48, 2999, 3000, 2999, 100}, // mad
{1, 1, 3000, 1, 100}, // question mad
{16, 824, 1501, 1951, 100}, // mad
{1, 2211, 1501, 1951, 59}, // mad surprised
{26, 903, 835, 869, 100}, // mad
{2, 1410, 666, 1252, 52}, // mad question
{3, 1410, 666, 1252, 79}, // mad
{71, 565, 666, 1252, 79}, // mad boop
{1, 768, 1478, 1951, 46}, // mad "oh"
                      };



// various internally used variables. no need to edit.
unsigned long prevMillis = 0;
unsigned long lastBump = 0;
unsigned long lastLightScan = 0;
boolean driveWheel = 0;
boolean imDriving = 0;
int myAnxiety = 500;
int myDriveCount = 0;
int mySpeed;
boolean findLight = 1;

Servo leftServo;
Servo rightServo;
void setup(){
  leftServo.attach(leftServoPin);
  leftServo.write(leftServoStop);
  rightServo.attach(rightServoPin);
  rightServo.write(rightServoStop);
 
  pinMode(frontFeeler, INPUT);
  digitalWrite(frontFeeler, HIGH);
  pinMode(rearFeeler, INPUT);
  digitalWrite(rearFeeler, HIGH);
  pinMode(speakerPin, OUTPUT);
  digitalWrite(speakerPin, LOW);
 
  pinMode(eyePin+14, INPUT);
  digitalWrite(eyePin+14, HIGH);
 
  lastBump = millis();
  Serial.begin(9600);
  randomSeed(analogRead(unusedAnalogPin));

  noise(noiseArray[0]);      // play default startup sound
}

void loop(){
  if(imDriving == 0){
    // do something before getting back to driving
    // this is where to update delay based on "feelings", do a trick, whistle for tidbits, etc...
   
    // if no bumps for one minute, be happier
    if((millis() - lastBump > 60000) and (myAnxiety > 250)){
      myAnxiety -= 10;
      lastBump = millis();
      printLog("lastBump happiness");
    }
   
    // change drive speed based on anxiety level
    // map() failed to live up to my expectations...  so it remains if/else
    if(myAnxiety > 750){
      mySpeed = 2;
    }else if((myAnxiety >=500) and (myAnxiety <= 750)){
      mySpeed = 1;
    }else{
      mySpeed = 0;
    }
   
    // do a trick here
    // only if it's been over ten seconds since startup (so it doesn't do a trick right away)
    // don't do tricks if too unhappy. Is 630 not too unhappy enough?
    if((myAnxiety <= 630) and (random(10) == random(10)) and (millis() > 10000)){
      printLog("!!!RANDOMOSITY!!!");
      noise(happyArray[random(happyArrayCount)]);
      int myRand = random(10);
      if(myRand == 1){
        // dance
        printLog("Dance");
        for(int i=0; i < 2; i++){
          rightServo.write(rightServoStop);
          leftServo.write(servoDriveTimes[0][2]);
          driveDelay(servoDriveTimes[0][0] * 2);
          leftServo.write(leftServoStop);
          rightServo.write(servoDriveTimes[0][2]);
          driveDelay(servoDriveTimes[0][1] * 2);
          rightServo.write(rightServoStop);
          noise(happyArray[random(happyArrayCount)]);
        }
      }else if(myRand == 2){
        //spin
        printLog("Spin");
        leftServo.write(112);
        rightServo.write(112);
        driveDelay(3000);
        leftServo.write(leftServoStop);
        rightServo.write(rightServoStop);
        noise(happyArray[random(happyArrayCount)]);
      }
    }
   
    // find bright spot
    if((findLight == 1) or (millis() - lastLightScan > 300000)){
      int prevBright = 1024;
      int brightLocation = 1;
     
      // check and spin 8 times
      for(int i=1; i<=8; i++){
        leftServo.write(spinSpeed);
        rightServo.write(spinSpeed);
        driveDelay(eighthSpinTime);
        leftServo.write(leftServoStop);
        rightServo.write(rightServoStop);
        delay(500);

        Serial.print("Position: ");
        Serial.print(i);
        Serial.print(" - Level: ");
        int myLevel = analogRead(eyePin);
        Serial.println(myLevel);
       
        if(myLevel < prevBright){
          prevBright = myLevel;
          brightLocation = i;
          Serial.print("New bright spot: ");
          Serial.println(brightLocation);
        }
        delay(500);
      }
     
      int myMulti = 0;
     
      if(brightLocation == 1){
        myMulti = 7;
      }else{
        myMulti = brightLocation-2;
      }
     
      if(myMulti > 0){
        for(int i=0; i<myMulti; i++){
          leftServo.write(spinSpeed);
          rightServo.write(spinSpeed);
          driveDelay(300);
          leftServo.write(leftServoStop);
          rightServo.write(rightServoStop);
          delay(50);
        }
      }
     
      findLight = 0;
      lastLightScan = millis();
    }

    // more unhappy == more slow to return to driving
    delay(myAnxiety);
   
    // get back to driving
    imDriving = 1;
    prevMillis = millis();
  }else if(imDriving == 1){
    drive();
  }
}

// wrapped in a function so feelers can be checked while dancing/spinning
void driveDelay(int driveLength){
  for(int i=0; i<=driveLength; i+=25){
    delay(25);
    if(readFeelers() > 0) break;
  }
}

void drive(){
  int feelerState = readFeelers();
  if(feelerState == 0){
    // things are ok, check time, set appropriate wheel to drive
    if(millis() - prevMillis > servoDriveTimes[mySpeed][driveWheel]){
      driveWheel = !driveWheel;
      leftServo.write(leftServoStop);
      rightServo.write(rightServoStop);
      imDriving = 0;
      if((myDriveCount >= 4) and (myAnxiety > 250)){
        if((myAnxiety < 630) and (random(5) == 3)){
          noise(happyArray[random(happyArrayCount)]);
        }
        myDriveCount = 0;
        myAnxiety -= 10;
        printLog("driveCount happiness");
      }else{
        myDriveCount++;
      }
    }
  }else{
    driveWheel = !driveWheel;
    mySpeed = 0;
   
    noise(sadArray[random(sadArrayCount)]);
    prevMillis = millis();
  }
 
  if((driveWheel == 0) and (imDriving == 1)){
    leftServo.write(servoDriveTimes[mySpeed][2]);
    rightServo.write(rightServoStop);
  }else if((driveWheel == 1) and (imDriving == 1)){
    leftServo.write(leftServoStop);
    rightServo.write(servoDriveTimes[mySpeed][2]);
  }
 
}

int readFeelers(){
  int feeler1 = digitalRead(frontFeeler);
  int feeler2 = digitalRead(rearFeeler);
  int theReturn;
 
  if(feeler1 == LOW){
    delay(12);
    if(digitalRead(frontFeeler) == LOW) theReturn = 1;
  }else if(feeler2 == LOW){
    delay(12);
    if(digitalRead(rearFeeler) == LOW) theReturn = 2;
  }else{
    theReturn = 0;
  }
 
  if((theReturn > 0) and (myAnxiety < 1000)){
    myAnxiety += 10;
    myDriveCount = 0;
    printLog("feeler unhappiness");
    // if this bump is < 10 seconds since the last one, be angry
    if(millis() - lastBump < 10000){
      myAnxiety += 10;
      noise(angryArray[random(angryArrayCount)]);
      printLog("lastBump unhappiness");
    }
    lastBump = millis();
  }
  return theReturn;
}

void noise(int inArray[5]){
  int dur = inArray[0];
  int start = inArray[1];
  int mid = inArray[2];
  int finish = inArray[3];
  int step = inArray[4];

  if(start < mid){
    for(int i = start; i <= mid; i+=step){
      play(dur, i, i);
    }
    for(int i=mid; i >= finish; i-=step){
      play(dur, i, i);
    }
  }else if(start > mid){
    for(int i=start; i >= mid; i-=step){
      play(dur, i, i);
    }
    for(int i=mid; i <= finish; i+=step){
      play(dur, i, i);
    }
  }
}

void play(int dur, int start, int finish){
  for(int i=0; i<=dur; i++){
    digitalWrite(speakerPin, HIGH);
    delayMicroseconds(start);
    digitalWrite(speakerPin, LOW);
    delayMicroseconds(finish);
  }
}

void printLog(char localString[]){
  if(_DEBUG == 1){
  Serial.print(localString);
  Serial.print(" -- Anxiety: ");
  Serial.print(myAnxiety);
  Serial.print(" - mySpeed: ");
  Serial.println(mySpeed);
  Serial.print("millis: ");
  Serial.print(millis());
  Serial.print(" - lastBump: ");
  Serial.println(lastBump);
  }
}