File content as of revision 4:2b47356f4b7d:

#include "nervousPuppy.h"

/**
 *  Constructor - contains running loop
 */
 
Servo tilt(p21);
Servo rotate(p22);      
AnalogIn ain(p20);

C12832 lcd(p5, p7, p6, p8, p11);


nervousPuppy::nervousPuppy(){
    bool isRunning = true;
    while(isRunning){
        playerDistance = ain.read();
        
        lcd.cls();
        lcd.locate(0,3);
        lcd.printf("%f",playerDistance);
        
        if(shutdown()){//TurnOff
            //isRunning = !isRunning; 
        } else if(isScared()){//MoveBack
            scared = true;
            playerError = playerDistance - SCARED;
            lcd.printf(" TOO CLOSE");
            wait(2.0);
            if(calculateAngle("Vertical") != 0)changePosition("tilt",calculateAngle("Vertical"));
            else if(calculateAngle("Horizontal") != 0)changePosition("rotate",calculateAngle("Horizontal"));

            scared = false;
        } else if(isLonely()){// MoveForward
            lonely = true;
            playerError = playerDistance - LONELY;
            lcd.printf(" TOO FAR");
            wait(2.0);
            if(calculateAngle("Vertical") !=0)changePosition("tilt",calculateAngle("Vertical"));
            else if(calculateAngle("Horizontal") != 0)changePosition("rotate",calculateAngle("Horizontal"));
            
            lonely = false;
        }
    }
}

/**
 * Calculates the angle required to bring the 'puppy' to a 'safe distance'
 * Returns 0 if it cannot find a 'safe distance'
 */
float nervousPuppy::calculateAngle(string axis){
    float deltaDist = 0.0;
    float limiter,y;
    if(axis == "Vertical") limiter = SERVO_TILT_LIMIT;
    else limiter = SERVO_ROTATE_LIMIT;
    
    for(float theta = 0.0; theta < limiter; ++theta){
        y = RADIUS*Sin(theta);
        deltaDist = sqrt(pow(y,2) + pow(playerDistance,2));
        if(scared){
            if((deltaDist - SCARED) > SCARED) return theta;
        }else if(lonely){
            if((deltaDist - LONELY) < LONELY) return theta;
        }
    }
    return 0.0;
}

/**
 * Move 'puppy' to the calculated 'safe' point
 */
void nervousPuppy::changePosition(string servo,float angle){
    if(servo == "tilt"){
        tilt.position(angle);
    } else if(servo == "rotate"){
        rotate.position(angle);
    }
}
/**
 * Thread -> Running sonar to detect player
 */
void nervousPuppy::detectPlayer(){
    
}

/** check if we shutdown **/
bool nervousPuppy::shutdown(){
    if(playerDistance < SHUTDOWN)return true;
    else return false;   
}

/** check if player is to far away **/
bool nervousPuppy::isLonely(){
    if(playerDistance > LONELY)return true;
    else return false;    
}

/** check if player is to close **/
bool nervousPuppy::isScared(){
    if(playerDistance < SCARED)return true;
    else return false;
}

/** get player distance value **/
float nervousPuppy::getPlayerDistance(){
    return playerDistance;
}

/** set player distance value **/
void nervousPuppy::setPlayerDistance(float dist){
    playerDistance = dist;
}

int main(){
    nervousPuppy();
}