Sprint One / NervousPuppySprintOne

Fixed Sonar

Dependencies:   C12832 Servo mbed-rtos-edited mbed

Fork of NervousPuppy by Sean Doyle

nervousPuppy.cpp@2:8415bea33a95, 2015-01-13 (annotated)

Committer:
SeanDoyle
Date:
Tue Jan 13 16:08:22 2015 +0000
Revision:
2:8415bea33a95
Parent:
1:8fe6802d6971
Child:
3:74dfce05dd99
Implemented some logic for changing position.

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