Fixed Sonar
Dependencies: C12832 Servo mbed-rtos-edited mbed
Fork of NervousPuppy by
Diff: nervousPuppy.cpp
- Revision:
- 6:f854aa2f41e2
- Parent:
- 5:cbb5d7460309
- Child:
- 7:8b2cf5e6e888
--- a/nervousPuppy.cpp Mon Jan 19 13:31:42 2015 +0000 +++ b/nervousPuppy.cpp Mon Jan 26 14:50:41 2015 +0000 @@ -6,116 +6,39 @@ Servo tilt(p21); Servo rotate(p22); -AnalogIn ain(p20); +AnalogIn ainLeft(p15); +AnalogIn ainRight(p16); -C12832 lcd(p5, p7, p6, p8, p11); +C12832 lcd(p5, p7, +p6, p8, p11); nervousPuppy::nervousPuppy(){ - bool isRunning = true; - while(isRunning){ - playerDistance = 254*ain.read(); + rotation = 0; + while(1){ + + float x = ainRight.read(); + float y = ainLeft.read(); + lcd.cls(); - if(shutdown()){//TurnOff - //isRunning = !isRunning; - } else if(isScared()){//MoveBack - scared = true; - playerError = playerDistance - SCARED; - lcd.printf(" TOO CLOSE"); - wait(2.0); - - - scared = false; - } else if(isLonely()){// MoveForward - lonely = true; - playerError = playerDistance - LONELY; - lcd.printf(" TOO FAR"); - wait(2.0); - - - lonely = false; + lcd.locate(0,3); + lcd.printf("x = %f",x); + lcd.printf(" y = %f",y); + + + if(x > 0.10 && y < 0.10){ + lcd.printf("rotating by 2"); + rotate.calibrate(0.0005,2); + }else if(y > 0.10 && x < 0.10){ + lcd.printf("rotating by -2"); + rotate.calibrate(0.0005,-2); } + wait(0.2); } } -/** - * 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 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; - } - } - } - 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; -} -/** - * Move 'puppy' to the calculated 'safe' point - */ -void nervousPuppy::changePosition(string servo,float angle){ - if(servo == "tilt"){ - tilt.position(angle); - } else if(servo == "rotate"){ - rotate.position(angle); - } -} -/** - * 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; -} int main(){ nervousPuppy();