Code with mutex and threads
Dependencies: C12832 Servo mbed-rtos-edited mbed
Fork of NervousPuppySprintOne by
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?
User | Revision | Line number | New 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 | } |