Sprint One / NervousPuppySprintOne

Fixed Sonar

Dependencies:   C12832 Servo mbed-rtos-edited mbed

Fork of NervousPuppy by Sean Doyle

nervousPuppy.cpp@4:2b47356f4b7d, 2015-01-15 (annotated)

Committer:
SeanDoyle
Date:
Thu Jan 15 17:01:47 2015 +0000
Revision:
4:2b47356f4b7d
Parent:
3:74dfce05dd99
Child:
5:cbb5d7460309
Sonar working;

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