Herbert the Digidog

/*
Mood & Energy Methods

Will contain all the methods that control Herbet's mood and energy. 
*/

// Health Variables
extern int moodRedPin = 10;            // LED red pin for Eyes
extern int moodGreenPin = 11;          // LED green pin for Eyes
extern int moodBluePin = 12;           // LED Blue pin for Eyes
extern float moodValue = 65;          // current Mood value (0 - 100)
extern float moodChangeFactor = 0.1;   // degrade mood by this much over time with each health interval
extern float energyChangeFactor = 0.1; // degrade by this much over time with each health interval
extern float energyValue = 100;        // current Energy (0 - 100)
extern float foodChangeFactor = 2.0;   // increase Energy by this much over time when eating
extern boolean isSleeping = false;     // set to true if currently sleeping

void setupMoodAndEnergy()
{
  pinMode(moodRedPin, OUTPUT);
  pinMode(moodGreenPin, OUTPUT);
  pinMode(moodBluePin, OUTPUT);  
}

void determineMood()
{
    if (moodValue >= 90)
    {
      setColor(0,energyValue * 2.55,0); //green
    }
    else if (moodValue >= 70 && moodValue < 90)
    {
      setColor(energyValue * 2.55, 0, energyValue * 2.55); //purple
    }
    else if (moodValue >= 60 && moodValue < 70)
    {
      setColor(0, 0, energyValue * 2.55); //blue
    }
    else if (moodValue >= 50 && moodValue < 60)
    {
      setColor(energyValue * 2.55, energyValue * 2.55, 0); //yellow
    }
    else if (moodValue >= 40 && moodValue < 50)
    {
      setColor(energyValue * 2.55, energyValue * 1.02, energyValue * 1.78); //orange
    }
    else if (moodValue < 40)
    {
      setColor(energyValue * 2.55, 0, 0); //red
    }
}

void adjustMood()
{
    moodValue = moodValue - moodChangeFactor;  //time degrades mood
    //if isSleeping then increase moodValue by a factor
    //if isFetching then increase moodValue by a factor
    //if isEating then increase moodValue by a factor
    if (isPetting == true)
    {
      moodValue = moodValue + .5;
    }
    if (isFetching == true)
    {
      moodValue = moodValue + .3;
    }
    if (isEating == true)
    {
      moodValue = moodValue + .2;
    }
    if (moodValue < 0)
    {
      moodValue = 0;
    }
    if (moodValue > 100)
    {
      moodValue = 100;
    }
    //Serial.print("mood: ");
    //Serial.println(moodValue);
}

void  adjustEnergy()
{
    energyValue = energyValue - energyChangeFactor;  //time degrades energy
    //if isSleeping then increase energyValue by a factor
    //if isFetching then decrease energyValue by a factor
    //if isEating then increase energyValue by a factor
    //if isPetting has no impact on energy
    if (isEating == true)
    {
      energyValue = energyValue + foodChangeFactor;
    }
    if (isFetching == true)
    {
      energyValue = energyValue - 0.2;
    }
    if (energyValue < 0)
    {
      energyValue = 0;
    }
    if (energyValue > 100)
    {
      energyValue = 100;
    }
    //Serial.print("Energy: ");
    //Serial.println(energyValue);
}

void checkForFood()
{
  static int food = 0;
  //uint16_t blocks = 0;
  
  //blocks = pixy.getBlocks();  // commented out for debugging without camera
  
  if (blocks)
  {
    if (pixy.blocks[food].signature > 0)
    {
      isEating = true;
    }
  }
}

void setColor(int red, int green, int blue)
{
  /*Serial.print("LED change: ");
  Serial.print(red);
  Serial.print(", ");
  Serial.print(green);
  Serial.print(", ");
  Serial.println(blue); */
  analogWrite(moodRedPin, red);
  analogWrite(moodGreenPin, green);
  analogWrite(moodBluePin, blue);  
}

/*
Motor Driver Methods

Will contain all the methods Herbet uses to move about. 
*/

void MotorSpeedSetAB(unsigned char MotorSpeedA , unsigned char MotorSpeedB)  {
  MotorSpeedA=map(MotorSpeedA,0,100,0,255);
  MotorSpeedB=map(MotorSpeedB,0,100,0,255);
  Wire.beginTransmission(I2CMotorDriverAdd);
  Wire.write(MotorSpeedSet);       
  Wire.write(MotorSpeedA);             
  Wire.write(MotorSpeedB);              
  Wire.endTransmission();    
}
//set the prescale frequency of PWM, 0x03 default;
void MotorPWMFrequenceSet(unsigned char Frequence)  {    
  Wire.beginTransmission(I2CMotorDriverAdd); 
  Wire.write(PWMFrequenceSet);       
  Wire.write(Frequence);             
  Wire.write(Nothing);            
  Wire.endTransmission();   
}
//set the direction of DC motor. 
void MotorDirectionSet(unsigned char Direction)  {     
  Wire.beginTransmission(I2CMotorDriverAdd);
  Wire.write(DirectionSet);        
  Wire.write(Direction);             
  Wire.write(Nothing);             
  Wire.endTransmission();    
}

void MotorDriectionAndSpeedSet(unsigned char Direction,unsigned char MotorSpeedA,unsigned char MotorSpeedB)  {  
  MotorDirectionSet(Direction);
  MotorSpeedSetAB(MotorSpeedA,MotorSpeedB);  
}

int getSpeed(int xValue)
{
  static int speedCap = 25;
  static int offSet = 195;
  int returnValue = 0;
  
  returnValue = xValue - offSet;
  
  if (returnValue < 0)
  {
    returnValue = returnValue - returnValue - returnValue;
  }
  
  if (returnValue > speedCap)
  {
    returnValue = speedCap;
  }
  return returnValue;
}

/*
Play Methods.

Will contain all the methods Herbet uses to play different games. 
*/

/*
pixy.blocks[i].signature The signature number of the detected object (1-7)
pixy.blocks[i].x The x location of the center of the detected object (0 to 319)
pixy.blocks[i].y The y location of the center of the detected object (0 to 199)
pixy.blocks[i].width The width of the detected object (1 to 320)
pixy.blocks[i].height The height of the detected object (1 to 200)
pixy.blocks[i].print() A member function that prints the detected object information to the serial port
*/

void fetchBall()
{   
 //Pixy test
  static int i = 0;
  static int ball = 1;
  static int minRange = 120;
  static int maxRange = 220;
  static int turnSpeed = 50;
  
  int j;
  //uint16_t blocks = 0;
  char buf[32]; 

  //blocks = pixy.getBlocks();
  
  if (blocks)
  {
    //Serial.println(pixy.blocks[ball].width);
    sprintf(buf, "Detected %d:\n", blocks);
    Serial.println(buf);
    Serial.println(pixy.blocks[ball].x);
    pixy.blocks[ball].print();
    MotorSpeedSetAB(0,0);
    delay(10);
    MotorDirectionSet(0b0000);
    
    if (pixy.blocks[ball].width >= 50) // stop fetching once it is close enough to have "caught it"
    {
      isFetching = false;
      Serial.println("done fetching the ball.");  
      MotorSpeedSetAB(0,0);
      delay(10);
    }
    else if (pixy.blocks[ball].x > maxRange)
    {
       Serial.println("turn right");
       //MotorSpeedSetAB(getSpeed(pixy.blocks[ball].x),getSpeed(pixy.blocks[ball].x));
       MotorSpeedSetAB(5, turnSpeed);
       delay(20); 
       MotorDirectionSet(0b1010);  
    }
    else if (pixy.blocks[ball].x < minRange)
    {
       Serial.println("turn left");
       //MotorSpeedSetAB(getSpeed(pixy.blocks[ball].x),getSpeed(pixy.blocks[ball].x));
       MotorSpeedSetAB(turnSpeed, 5);
       delay(20); 
       MotorDirectionSet(0b1010);                           
    }
    else if (pixy.blocks[ball].width < 50)
    {
      Serial.println("FORWARDS");
      MotorSpeedSetAB(80,80);
      delay(10);
      MotorDirectionSet(0b1010);      
    } 

    // run the set direction for a bit then stop motor
    delay(300); 
    MotorSpeedSetAB(0,0);
  }
  else
  {
    MotorSpeedSetAB(0,0);
  }
}