Diff: nervousPuppy.cpp

Revision:

2:8415bea33a95

Parent:

1:8fe6802d6971

Child:

3:74dfce05dd99

--- a/nervousPuppy.cpp	Mon Jan 12 19:32:28 2015 +0000
+++ b/nervousPuppy.cpp	Tue Jan 13 16:08:22 2015 +0000
@@ -1,27 +1,106 @@
 #include "nervousPuppy.h"
 
+/**
+ *  Constructor - contains running loop
+ */
 nervousPuppy::nervousPuppy(){
     bool isRunning = true;
     while(isRunning){
-        if(isLonely()){} // MoveForward
-        else if(isScared()){} //MoveBack
+        if(shutdown()){//TurnOff
+            isRunning = !isRunning; 
+        } else if(isScared()){//MoveBack
+            scared = true;
+            playerError = playerDistance - SCARED;
+            
+            if(calculateVerticalAdjustment() != 0)changePosition("tilt",calculateVerticalAdjustment());
+            else changePosition("rotate",calculateHorizontalAdjustment());
+            scared = false;
+        } else if(isLonely()){// MoveForward
+            lonely = true;
+            playerError = playerDistance - LONELY;
+            
+            if(calculateVerticalAdjustment() !=0)changePosition("tilt",calculateVerticalAdjustment());
+            else changePosition("rotate",calculateHorizontalAdjustment());
+            lonely = false;
+        }
     }
 }
 
+/**
+ * Returns the vertical adjustment
+ */
+float nervousPuppy::calculateVerticalAdjustment(){
+    return calculateAngle("Vertical");
+}
+
+/**
+ * Returns the horizontal adjustment
+ */
+float nervousPuppy::calculateHorizontalAdjustment(){
+    return calculateAngle("Horizontal");
+}
+
+/**
+ * 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"){}
+    else if(servo == "rotate"){}
+}
+
+/**
+ * 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;
 }