Sprint One / NervousPuppySprintOne

Fixed Sonar

Dependencies:   C12832 Servo mbed-rtos-edited mbed

Fork of NervousPuppy by Sean Doyle

nervousPuppy.cpp@3:74dfce05dd99, 2015-01-14 (annotated)

Committer:
SeanDoyle
Date:
Wed Jan 14 16:57:41 2015 +0000
Revision:
3:74dfce05dd99
Parent:
2:8415bea33a95
Child:
4:2b47356f4b7d
Tidied code, fixed issue for no optimal angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SeanDoyle 0:f71b200c2f4d 1 #include "nervousPuppy.h"
SeanDoyle 0:f71b200c2f4d 2
SeanDoyle 2:8415bea33a95 3 /**
SeanDoyle 2:8415bea33a95 4 * Constructor - contains running loop
SeanDoyle 2:8415bea33a95 5 */
SeanDoyle 0:f71b200c2f4d 6 nervousPuppy::nervousPuppy(){
SeanDoyle 1:8fe6802d6971 7 bool isRunning = true;
SeanDoyle 1:8fe6802d6971 8 while(isRunning){
SeanDoyle 2:8415bea33a95 9 if(shutdown()){//TurnOff
SeanDoyle 2:8415bea33a95 10 isRunning = !isRunning;
SeanDoyle 2:8415bea33a95 11 } else if(isScared()){//MoveBack
SeanDoyle 2:8415bea33a95 12 scared = true;
SeanDoyle 2:8415bea33a95 13 playerError = playerDistance - SCARED;
SeanDoyle 2:8415bea33a95 14
SeanDoyle 3:74dfce05dd99 15 if(calculateAngle("Vertical") != 0)changePosition("tilt",calculateAngle("Vertical"));
SeanDoyle 3:74dfce05dd99 16 else if(calculateAngle("Horizontal") != 0)changePosition("rotate",calculateAngle("Horizontal"));
SeanDoyle 3:74dfce05dd99 17 else isRunning = false;
SeanDoyle 3:74dfce05dd99 18
SeanDoyle 2:8415bea33a95 19 scared = false;
SeanDoyle 2:8415bea33a95 20 } else if(isLonely()){// MoveForward
SeanDoyle 2:8415bea33a95 21 lonely = true;
SeanDoyle 2:8415bea33a95 22 playerError = playerDistance - LONELY;
SeanDoyle 2:8415bea33a95 23
SeanDoyle 3:74dfce05dd99 24 if(calculateAngle("Vertical") !=0)changePosition("tilt",calculateAngle("Vertical"));
SeanDoyle 3:74dfce05dd99 25 else if(calculateAngle("Horizontal") != 0)changePosition("rotate",calculateAngle("Horizontal"));
SeanDoyle 3:74dfce05dd99 26 else isRunning = false;
SeanDoyle 3:74dfce05dd99 27
SeanDoyle 2:8415bea33a95 28 lonely = false;
SeanDoyle 2:8415bea33a95 29 }
SeanDoyle 1:8fe6802d6971 30 }
SeanDoyle 1:8fe6802d6971 31 }
SeanDoyle 1:8fe6802d6971 32
SeanDoyle 2:8415bea33a95 33 /**
SeanDoyle 2:8415bea33a95 34 * Calculates the angle required to bring the 'puppy' to a 'safe distance'
SeanDoyle 2:8415bea33a95 35 * Returns 0 if it cannot find a 'safe distance'
SeanDoyle 2:8415bea33a95 36 */
SeanDoyle 2:8415bea33a95 37 float nervousPuppy::calculateAngle(string axis){
SeanDoyle 2:8415bea33a95 38 float deltaDist = 0.0;
SeanDoyle 2:8415bea33a95 39 float limiter,y;
SeanDoyle 2:8415bea33a95 40 if(axis == "Vertical") limiter = SERVO_TILT_LIMIT;
SeanDoyle 2:8415bea33a95 41 else limiter = SERVO_ROTATE_LIMIT;
SeanDoyle 2:8415bea33a95 42
SeanDoyle 2:8415bea33a95 43 for(float theta = 0.0; theta < limiter; ++theta){
SeanDoyle 2:8415bea33a95 44 y = RADIUS*Sin(theta);
SeanDoyle 2:8415bea33a95 45 deltaDist = sqrt(pow(y,2) + pow(playerDistance,2));
SeanDoyle 2:8415bea33a95 46 if(scared){
SeanDoyle 2:8415bea33a95 47 if((deltaDist - SCARED) > SCARED) return theta;
SeanDoyle 2:8415bea33a95 48 }else if(lonely){
SeanDoyle 2:8415bea33a95 49 if((deltaDist - LONELY) < LONELY) return theta;
SeanDoyle 2:8415bea33a95 50 }
SeanDoyle 2:8415bea33a95 51 }
SeanDoyle 2:8415bea33a95 52 return 0.0;
SeanDoyle 2:8415bea33a95 53 }
SeanDoyle 2:8415bea33a95 54
SeanDoyle 2:8415bea33a95 55 /**
SeanDoyle 2:8415bea33a95 56 * Move 'puppy' to the calculated 'safe' point
SeanDoyle 2:8415bea33a95 57 */
SeanDoyle 2:8415bea33a95 58 void nervousPuppy::changePosition(string servo,float angle){
SeanDoyle 2:8415bea33a95 59 if(servo == "tilt"){}
SeanDoyle 2:8415bea33a95 60 else if(servo == "rotate"){}
SeanDoyle 2:8415bea33a95 61 }
SeanDoyle 2:8415bea33a95 62
SeanDoyle 2:8415bea33a95 63 /**
SeanDoyle 2:8415bea33a95 64 * Thread -> Running sonar to detect player
SeanDoyle 2:8415bea33a95 65 */
SeanDoyle 2:8415bea33a95 66 void nervousPuppy::detectPlayer(){
SeanDoyle 2:8415bea33a95 67
SeanDoyle 2:8415bea33a95 68 }
SeanDoyle 2:8415bea33a95 69
SeanDoyle 2:8415bea33a95 70 /** check if we shutdown **/
SeanDoyle 2:8415bea33a95 71 bool nervousPuppy::shutdown(){
SeanDoyle 2:8415bea33a95 72 if(playerDistance < SHUTDOWN)return true;
SeanDoyle 2:8415bea33a95 73 else return false;
SeanDoyle 2:8415bea33a95 74 }
SeanDoyle 2:8415bea33a95 75
SeanDoyle 2:8415bea33a95 76 /** check if player is to far away **/
SeanDoyle 1:8fe6802d6971 77 bool nervousPuppy::isLonely(){
SeanDoyle 1:8fe6802d6971 78 if(playerDistance > LONELY)return true;
SeanDoyle 1:8fe6802d6971 79 else return false;
SeanDoyle 0:f71b200c2f4d 80 }
SeanDoyle 0:f71b200c2f4d 81
SeanDoyle 2:8415bea33a95 82 /** check if player is to close **/
SeanDoyle 1:8fe6802d6971 83 bool nervousPuppy::isScared(){
SeanDoyle 1:8fe6802d6971 84 if(playerDistance < SCARED)return true;
SeanDoyle 1:8fe6802d6971 85 else return false;
SeanDoyle 1:8fe6802d6971 86 }
SeanDoyle 2:8415bea33a95 87
SeanDoyle 2:8415bea33a95 88 /** get player distance value **/
SeanDoyle 1:8fe6802d6971 89 float nervousPuppy::getPlayerDistance(){
SeanDoyle 1:8fe6802d6971 90 return playerDistance;
SeanDoyle 1:8fe6802d6971 91 }
SeanDoyle 0:f71b200c2f4d 92
SeanDoyle 2:8415bea33a95 93 /** set player distance value **/
SeanDoyle 1:8fe6802d6971 94 void nervousPuppy::setPlayerDistance(float dist){
SeanDoyle 1:8fe6802d6971 95 playerDistance = dist;
SeanDoyle 1:8fe6802d6971 96 }
SeanDoyle 0:f71b200c2f4d 97
SeanDoyle 1:8fe6802d6971 98 int main(){
SeanDoyle 1:8fe6802d6971 99 nervousPuppy();
SeanDoyle 0:f71b200c2f4d 100 }