Revision 2:8415bea33a95, committed 2015-01-13

Comitter:

SeanDoyle

Date:

Tue Jan 13 16:08:22 2015 +0000

Parent:

1:8fe6802d6971

Child:

3:74dfce05dd99

Commit message:

Implemented some logic for changing position.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Tue Jan 13 16:08:22 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- a/mbed-rtos-edited.lib	Mon Jan 12 19:32:28 2015 +0000
+++ b/mbed-rtos-edited.lib	Tue Jan 13 16:08:22 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#3b7770ca2984
+http://developer.mbed.org/users/SeanDoyle/code/mbed-rtos-edited/#f535c468d377

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

--- a/nervousPuppy.h	Mon Jan 12 19:32:28 2015 +0000
+++ b/nervousPuppy.h	Tue Jan 13 16:08:22 2015 +0000
@@ -1,25 +1,42 @@
 #include "mbed.h"
+#include "rtos.h"
+#include "Servo.h"
+#include <math.h>
+#include <string>
 
 #define Cos(a) cos(PI/180*(a))
 #define Sin(a) sin(PI/180*(a))
 #define PI 3.1415926
 
-using namespace std;
-
-static const float LONELY = 5.0f;
-static const float SCARED = 0.5f;
-
 class nervousPuppy{
     public:
         nervousPuppy();
         
         bool isLonely();
         bool isScared();
+        bool shutdown();
         
+        void detectPlayer();
         float getPlayerDistance();
         void setPlayerDistance(float dist);
-                
+        
+        float calculateVerticalAdjustment();
+        float calculateHorizontalAdjustment();
+        float calculateAngle(string axis);
+        void changePosition(string servo,float angle); 
+        
     private:
-        float playerDistance;
-
+        //Servo tilt();
+        //Servo rotate();      
+        
+        Mutex lockForDistance; //Not sure why needed
+        float playerDistance, playerError;
+        bool scared, lonely;
+        const float LONELY = 10.0f; //Placeholder number
+        const float SCARED = 3.0f; //Placeholder number
+        const float RADIUS = 2.0f; //Placeholder number
+        const float SHUTDOWN = 1.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
+        const float SERVO_ROTATE_LIMIT = 45; //Placeholder number - max angle servo can rotate by
 };
\ No newline at end of file