Sprint One / NervousPuppySprint2602

Code with mutex and threads

Dependencies:   C12832 Servo mbed-rtos-edited mbed

Fork of NervousPuppySprintOne by Sprint One

nervousPuppy.cpp@5:cbb5d7460309, 2015-01-19 (annotated)

Committer:
SeanDoyle
Date:
Mon Jan 19 13:31:42 2015 +0000
Revision:
5:cbb5d7460309
Parent:
4:2b47356f4b7d
Child:
6:f854aa2f41e2
Changed Math;

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 4:2b47356f4b7d 6
SeanDoyle 4:2b47356f4b7d 7 Servo tilt(p21);
SeanDoyle 4:2b47356f4b7d 8 Servo rotate(p22);
SeanDoyle 4:2b47356f4b7d 9 AnalogIn ain(p20);
SeanDoyle 4:2b47356f4b7d 10
SeanDoyle 4:2b47356f4b7d 11 C12832 lcd(p5, p7, p6, p8, p11);
SeanDoyle 4:2b47356f4b7d 12
SeanDoyle 4:2b47356f4b7d 13
SeanDoyle 0:f71b200c2f4d 14 nervousPuppy::nervousPuppy(){
SeanDoyle 1:8fe6802d6971 15 bool isRunning = true;
SeanDoyle 1:8fe6802d6971 16 while(isRunning){
SeanDoyle 5:cbb5d7460309 17 playerDistance = 254*ain.read();
SeanDoyle 4:2b47356f4b7d 18
SeanDoyle 4:2b47356f4b7d 19 lcd.cls();
SeanDoyle 2:8415bea33a95 20 if(shutdown()){//TurnOff
SeanDoyle 4:2b47356f4b7d 21 //isRunning = !isRunning;
SeanDoyle 2:8415bea33a95 22 } else if(isScared()){//MoveBack
SeanDoyle 2:8415bea33a95 23 scared = true;
SeanDoyle 2:8415bea33a95 24 playerError = playerDistance - SCARED;
SeanDoyle 4:2b47356f4b7d 25 lcd.printf(" TOO CLOSE");
SeanDoyle 4:2b47356f4b7d 26 wait(2.0);
SeanDoyle 5:cbb5d7460309 27
SeanDoyle 4:2b47356f4b7d 28
SeanDoyle 2:8415bea33a95 29 scared = false;
SeanDoyle 2:8415bea33a95 30 } else if(isLonely()){// MoveForward
SeanDoyle 2:8415bea33a95 31 lonely = true;
SeanDoyle 2:8415bea33a95 32 playerError = playerDistance - LONELY;
SeanDoyle 4:2b47356f4b7d 33 lcd.printf(" TOO FAR");
SeanDoyle 4:2b47356f4b7d 34 wait(2.0);
SeanDoyle 5:cbb5d7460309 35
SeanDoyle 3:74dfce05dd99 36
SeanDoyle 2:8415bea33a95 37 lonely = false;
SeanDoyle 2:8415bea33a95 38 }
SeanDoyle 1:8fe6802d6971 39 }
SeanDoyle 1:8fe6802d6971 40 }
SeanDoyle 1:8fe6802d6971 41
SeanDoyle 2:8415bea33a95 42 /**
SeanDoyle 2:8415bea33a95 43 * Calculates the angle required to bring the 'puppy' to a 'safe distance'
SeanDoyle 2:8415bea33a95 44 * Returns 0 if it cannot find a 'safe distance'
SeanDoyle 2:8415bea33a95 45 */
SeanDoyle 5:cbb5d7460309 46 float nervousPuppy::calculateAngle(string issue){
SeanDoyle 5:cbb5d7460309 47 if(issue == "Too Close"){
SeanDoyle 5:cbb5d7460309 48 for(float theta = 0.0; theta < 45; theta++){
SeanDoyle 5:cbb5d7460309 49 float c = RADIUS*Cos(theta);
SeanDoyle 5:cbb5d7460309 50 float y = RADIUS*Sin(theta);
SeanDoyle 5:cbb5d7460309 51 float b = RADIUS - c;
SeanDoyle 5:cbb5d7460309 52
SeanDoyle 5:cbb5d7460309 53 float x = sqrt(pow(y,2) + pow((playerDistance+b),2));
SeanDoyle 5:cbb5d7460309 54 if(x > SCARED){
SeanDoyle 5:cbb5d7460309 55 return theta;
SeanDoyle 5:cbb5d7460309 56 }
SeanDoyle 2:8415bea33a95 57 }
SeanDoyle 2:8415bea33a95 58 }
SeanDoyle 5:cbb5d7460309 59 if(issue == "Too Far"){
SeanDoyle 5:cbb5d7460309 60 for(float theta = 0.0; theta < 45; theta++){
SeanDoyle 5:cbb5d7460309 61 float c = RADIUS*Cos(theta);
SeanDoyle 5:cbb5d7460309 62 float y = RADIUS*Sin(theta);
SeanDoyle 5:cbb5d7460309 63 float b = RADIUS - c;
SeanDoyle 5:cbb5d7460309 64 float e = playerDistance - b;
SeanDoyle 5:cbb5d7460309 65
SeanDoyle 5:cbb5d7460309 66 float a = sqrt(pow(e,2) + pow(y,2));
SeanDoyle 5:cbb5d7460309 67 if(a < LONELY){
SeanDoyle 5:cbb5d7460309 68 return theta;
SeanDoyle 5:cbb5d7460309 69 }
SeanDoyle 5:cbb5d7460309 70 }
SeanDoyle 5:cbb5d7460309 71 }
SeanDoyle 5:cbb5d7460309 72 return 0.0;
SeanDoyle 2:8415bea33a95 73 }
SeanDoyle 2:8415bea33a95 74
SeanDoyle 2:8415bea33a95 75 /**
SeanDoyle 2:8415bea33a95 76 * Move 'puppy' to the calculated 'safe' point
SeanDoyle 2:8415bea33a95 77 */
SeanDoyle 2:8415bea33a95 78 void nervousPuppy::changePosition(string servo,float angle){
SeanDoyle 4:2b47356f4b7d 79 if(servo == "tilt"){
SeanDoyle 4:2b47356f4b7d 80 tilt.position(angle);
SeanDoyle 4:2b47356f4b7d 81 } else if(servo == "rotate"){
SeanDoyle 4:2b47356f4b7d 82 rotate.position(angle);
SeanDoyle 4:2b47356f4b7d 83 }
SeanDoyle 2:8415bea33a95 84 }
SeanDoyle 2:8415bea33a95 85 /**
SeanDoyle 2:8415bea33a95 86 * Thread -> Running sonar to detect player
SeanDoyle 2:8415bea33a95 87 */
SeanDoyle 2:8415bea33a95 88 void nervousPuppy::detectPlayer(){
SeanDoyle 2:8415bea33a95 89
SeanDoyle 2:8415bea33a95 90 }
SeanDoyle 2:8415bea33a95 91
SeanDoyle 2:8415bea33a95 92 /** check if we shutdown **/
SeanDoyle 2:8415bea33a95 93 bool nervousPuppy::shutdown(){
SeanDoyle 2:8415bea33a95 94 if(playerDistance < SHUTDOWN)return true;
SeanDoyle 2:8415bea33a95 95 else return false;
SeanDoyle 2:8415bea33a95 96 }
SeanDoyle 2:8415bea33a95 97
SeanDoyle 2:8415bea33a95 98 /** check if player is to far away **/
SeanDoyle 1:8fe6802d6971 99 bool nervousPuppy::isLonely(){
SeanDoyle 1:8fe6802d6971 100 if(playerDistance > LONELY)return true;
SeanDoyle 1:8fe6802d6971 101 else return false;
SeanDoyle 0:f71b200c2f4d 102 }
SeanDoyle 0:f71b200c2f4d 103
SeanDoyle 2:8415bea33a95 104 /** check if player is to close **/
SeanDoyle 1:8fe6802d6971 105 bool nervousPuppy::isScared(){
SeanDoyle 1:8fe6802d6971 106 if(playerDistance < SCARED)return true;
SeanDoyle 1:8fe6802d6971 107 else return false;
SeanDoyle 1:8fe6802d6971 108 }
SeanDoyle 2:8415bea33a95 109
SeanDoyle 2:8415bea33a95 110 /** get player distance value **/
SeanDoyle 1:8fe6802d6971 111 float nervousPuppy::getPlayerDistance(){
SeanDoyle 1:8fe6802d6971 112 return playerDistance;
SeanDoyle 1:8fe6802d6971 113 }
SeanDoyle 0:f71b200c2f4d 114
SeanDoyle 2:8415bea33a95 115 /** set player distance value **/
SeanDoyle 1:8fe6802d6971 116 void nervousPuppy::setPlayerDistance(float dist){
SeanDoyle 1:8fe6802d6971 117 playerDistance = dist;
SeanDoyle 1:8fe6802d6971 118 }
SeanDoyle 0:f71b200c2f4d 119
SeanDoyle 1:8fe6802d6971 120 int main(){
SeanDoyle 1:8fe6802d6971 121 nervousPuppy();
SeanDoyle 0:f71b200c2f4d 122 }