Fixed Sonar
Dependencies: C12832 Servo mbed-rtos-edited mbed
Fork of NervousPuppy by
nervousPuppy.cpp
- Committer:
- SeanDoyle
- Date:
- 2015-01-15
- Revision:
- 4:2b47356f4b7d
- Parent:
- 3:74dfce05dd99
- Child:
- 5:cbb5d7460309
File content as of revision 4:2b47356f4b7d:
#include "nervousPuppy.h" /** * Constructor - contains running loop */ Servo tilt(p21); Servo rotate(p22); AnalogIn ain(p20); C12832 lcd(p5, p7, p6, p8, p11); nervousPuppy::nervousPuppy(){ bool isRunning = true; while(isRunning){ playerDistance = ain.read(); lcd.cls(); lcd.locate(0,3); lcd.printf("%f",playerDistance); if(shutdown()){//TurnOff //isRunning = !isRunning; } else if(isScared()){//MoveBack scared = true; 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 lonely = true; 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; } } } /** * 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"){ 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(); }