Revision 5:cbb5d7460309, committed 2015-01-19

Comitter:

SeanDoyle

Date:

Mon Jan 19 13:31:42 2015 +0000

Parent:

4:2b47356f4b7d

Child:

6:f854aa2f41e2

Commit message:

Changed Math;

Changed in this revision

--- 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;    
 }
 
 /**

--- a/nervousPuppy.h	Thu Jan 15 17:01:47 2015 +0000
+++ b/nervousPuppy.h	Mon Jan 19 13:31:42 2015 +0000
@@ -22,7 +22,7 @@
         float getPlayerDistance();
         void setPlayerDistance(float dist);
         
-        float calculateAngle(string axis);
+        float calculateAngle(string issue);
         void changePosition(string servo,float angle); 
         
     private:
@@ -30,9 +30,9 @@
         Mutex lockForDistance; //Not sure why needed
         float playerDistance, playerError;
         bool scared, lonely;
-        const float LONELY = 0.10f; //Placeholder number
-        const float SCARED = 0.088f; //Placeholder number
-        const float RADIUS = 2.0f; //Placeholder number
+        const float LONELY = 3.0f; //Placeholder number
+        const float SCARED = 20.0f; //Placeholder number
+        const float RADIUS = 1.75f; //Placeholder number
         const float SHUTDOWN = 0.0f; //Placeholder number
         const float PULSE_RATE = 10; //Placeholder number - Pulses per second
         const float SERVO_TILT_LIMIT = 45; //Placeholder number - max angle servo can tilt by