Fixed Sonar
Dependencies: C12832 Servo mbed-rtos-edited mbed
Fork of NervousPuppy by
Diff: nervousPuppy.cpp
- Revision:
- 2:8415bea33a95
- Parent:
- 1:8fe6802d6971
- Child:
- 3:74dfce05dd99
--- 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; }