Diff: nervousPuppy.cpp

Revision:

5:cbb5d7460309

Parent:

4:2b47356f4b7d

Child:

6:f854aa2f41e2

--- a/nervousPuppy.cpp	Thu Jan 15 17:01:47 2015 +0000
+++ b/nervousPuppy.cpp	Mon Jan 19 13:31:42 2015 +0000
@@ -14,12 +14,9 @@
 nervousPuppy::nervousPuppy(){
     bool isRunning = true;
     while(isRunning){
-        playerDistance = ain.read();
+        playerDistance = 254*ain.read();
         
         lcd.cls();
-        lcd.locate(0,3);
-        lcd.printf("%f",playerDistance);
-        
         if(shutdown()){//TurnOff
             //isRunning = !isRunning; 
         } else if(isScared()){//MoveBack
@@ -27,8 +24,7 @@
             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
@@ -36,8 +32,7 @@
             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;
         }
@@ -48,22 +43,33 @@
  * 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;
+float nervousPuppy::calculateAngle(string issue){
+    if(issue == "Too Close"){
+        for(float theta = 0.0; theta < 45; theta++){
+            float c = RADIUS*Cos(theta);
+            float y = RADIUS*Sin(theta);
+            float b = RADIUS - c;
+           
+            float x = sqrt(pow(y,2) + pow((playerDistance+b),2));
+            if(x > SCARED){
+               return theta;
+            }
         }
     }
-    return 0.0;
+    if(issue == "Too Far"){
+        for(float theta = 0.0; theta < 45; theta++){
+            float c = RADIUS*Cos(theta);
+            float y = RADIUS*Sin(theta);
+            float b = RADIUS - c;
+            float e = playerDistance - b;
+            
+            float a = sqrt(pow(e,2) + pow(y,2));
+            if(a < LONELY){
+                return theta;   
+            }
+        }
+    }
+    return 0.0;    
 }
 
 /**