Code with mutex and threads
Dependencies: C12832 Servo mbed-rtos-edited mbed
Fork of NervousPuppySprintOne by
Diff: nervousPuppy.cpp
- Revision:
- 5:cbb5d7460309
- Parent:
- 4:2b47356f4b7d
- Child:
- 6:f854aa2f41e2
--- a/nervousPuppy.cpp Thu Jan 15 17:01:47 2015 +0000 +++ b/nervousPuppy.cpp Mon Jan 19 13:31:42 2015 +0000 @@ -14,12 +14,9 @@ nervousPuppy::nervousPuppy(){ bool isRunning = true; while(isRunning){ - playerDistance = ain.read(); + playerDistance = 254*ain.read(); lcd.cls(); - lcd.locate(0,3); - lcd.printf("%f",playerDistance); - if(shutdown()){//TurnOff //isRunning = !isRunning; } else if(isScared()){//MoveBack @@ -27,8 +24,7 @@ playerError = playerDistance - SCARED; lcd.printf(" TOO CLOSE"); wait(2.0); - if(calculateAngle("Vertical") != 0)changePosition("tilt",calculateAngle("Vertical")); - else if(calculateAngle("Horizontal") != 0)changePosition("rotate",calculateAngle("Horizontal")); + scared = false; } else if(isLonely()){// MoveForward @@ -36,8 +32,7 @@ playerError = playerDistance - LONELY; lcd.printf(" TOO FAR"); wait(2.0); - if(calculateAngle("Vertical") !=0)changePosition("tilt",calculateAngle("Vertical")); - else if(calculateAngle("Horizontal") != 0)changePosition("rotate",calculateAngle("Horizontal")); + lonely = false; } @@ -48,22 +43,33 @@ * 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; +float nervousPuppy::calculateAngle(string issue){ + if(issue == "Too Close"){ + for(float theta = 0.0; theta < 45; theta++){ + float c = RADIUS*Cos(theta); + float y = RADIUS*Sin(theta); + float b = RADIUS - c; + + float x = sqrt(pow(y,2) + pow((playerDistance+b),2)); + if(x > SCARED){ + return theta; + } } } - return 0.0; + if(issue == "Too Far"){ + for(float theta = 0.0; theta < 45; theta++){ + float c = RADIUS*Cos(theta); + float y = RADIUS*Sin(theta); + float b = RADIUS - c; + float e = playerDistance - b; + + float a = sqrt(pow(e,2) + pow(y,2)); + if(a < LONELY){ + return theta; + } + } + } + return 0.0; } /**