Diff: nervousPuppy.cpp

Revision:

4:2b47356f4b7d

Parent:

3:74dfce05dd99

--- a/nervousPuppy.cpp	Wed Jan 14 16:57:41 2015 +0000
+++ b/nervousPuppy.cpp	Thu Jan 15 17:01:47 2015 +0000
@@ -3,27 +3,41 @@
 /**
  *  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; 
+            //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"));
-            else isRunning = false;
-            
+
             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"));
-            else isRunning = false;
             
             lonely = false;
         }
@@ -56,10 +70,12 @@
  * Move 'puppy' to the calculated 'safe' point
  */
 void nervousPuppy::changePosition(string servo,float angle){
-    if(servo == "tilt"){}
-    else if(servo == "rotate"){}
+    if(servo == "tilt"){
+        tilt.position(angle);
+    } else if(servo == "rotate"){
+        rotate.position(angle);
+    }
 }
-
 /**
  * Thread -> Running sonar to detect player
  */