Can Kabuloglu / Mbed 2 deprecated BluetoothControlledSonarRobot

ECE 4180 - Mid mini project, lab 4

Dependencies:   Motor Servo mbed

main.cpp@0:d9d5943dfa45, 2017-03-13 (annotated)

Committer:
ckabuloglu
Date:
Mon Mar 13 16:08:11 2017 +0000
Revision:
0:d9d5943dfa45
intial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ckabuloglu 0:d9d5943dfa45 1 #include "mbed.h"
ckabuloglu 0:d9d5943dfa45 2 #include "Motor.h"
ckabuloglu 0:d9d5943dfa45 3 #include "Speaker.h"
ckabuloglu 0:d9d5943dfa45 4 #include "Servo.h"
ckabuloglu 0:d9d5943dfa45 5
ckabuloglu 0:d9d5943dfa45 6 DigitalOut led1(LED1);
ckabuloglu 0:d9d5943dfa45 7 DigitalOut led2(LED2);
ckabuloglu 0:d9d5943dfa45 8 DigitalOut led3(LED3);
ckabuloglu 0:d9d5943dfa45 9 DigitalOut led4(LED4);
ckabuloglu 0:d9d5943dfa45 10
ckabuloglu 0:d9d5943dfa45 11 Serial pc(USBTX, USBRX); // tx, rx
ckabuloglu 0:d9d5943dfa45 12
ckabuloglu 0:d9d5943dfa45 13 DigitalIn safe(p9);
ckabuloglu 0:d9d5943dfa45 14
ckabuloglu 0:d9d5943dfa45 15 Serial blue(p28,p27);
ckabuloglu 0:d9d5943dfa45 16
ckabuloglu 0:d9d5943dfa45 17 Motor motor_left(p23, p6, p5);
ckabuloglu 0:d9d5943dfa45 18 Motor motor_right(p24, p8, p7);
ckabuloglu 0:d9d5943dfa45 19
ckabuloglu 0:d9d5943dfa45 20 DigitalOut trigger(p13);
ckabuloglu 0:d9d5943dfa45 21 DigitalIn echo(p14);
ckabuloglu 0:d9d5943dfa45 22
ckabuloglu 0:d9d5943dfa45 23 Speaker mySpeaker(p18);
ckabuloglu 0:d9d5943dfa45 24
ckabuloglu 0:d9d5943dfa45 25 Servo servo(p26);
ckabuloglu 0:d9d5943dfa45 26
ckabuloglu 0:d9d5943dfa45 27 Timer sonar;
ckabuloglu 0:d9d5943dfa45 28
ckabuloglu 0:d9d5943dfa45 29 void chooseLed(int num) {
ckabuloglu 0:d9d5943dfa45 30 if (num == 1) {
ckabuloglu 0:d9d5943dfa45 31 led1 = 1;
ckabuloglu 0:d9d5943dfa45 32 led2 = 0;
ckabuloglu 0:d9d5943dfa45 33 led3 = 0;
ckabuloglu 0:d9d5943dfa45 34 led4 = 0;
ckabuloglu 0:d9d5943dfa45 35 } else if (num == 2) {
ckabuloglu 0:d9d5943dfa45 36 led1 = 0;
ckabuloglu 0:d9d5943dfa45 37 led2 = 1;
ckabuloglu 0:d9d5943dfa45 38 led3 = 0;
ckabuloglu 0:d9d5943dfa45 39 led4 = 0;
ckabuloglu 0:d9d5943dfa45 40 } else if (num == 3) {
ckabuloglu 0:d9d5943dfa45 41 led1 = 0;
ckabuloglu 0:d9d5943dfa45 42 led2 = 0;
ckabuloglu 0:d9d5943dfa45 43 led3 = 1;
ckabuloglu 0:d9d5943dfa45 44 led4 = 0;
ckabuloglu 0:d9d5943dfa45 45 } else if (num == 4) {
ckabuloglu 0:d9d5943dfa45 46 led1 = 0;
ckabuloglu 0:d9d5943dfa45 47 led2 = 0;
ckabuloglu 0:d9d5943dfa45 48 led3 = 0;
ckabuloglu 0:d9d5943dfa45 49 led4 = 1;
ckabuloglu 0:d9d5943dfa45 50 }
ckabuloglu 0:d9d5943dfa45 51 }
ckabuloglu 0:d9d5943dfa45 52
ckabuloglu 0:d9d5943dfa45 53
ckabuloglu 0:d9d5943dfa45 54 int getSonar(int correction) {
ckabuloglu 0:d9d5943dfa45 55 int distance = 0;
ckabuloglu 0:d9d5943dfa45 56 trigger = 1;
ckabuloglu 0:d9d5943dfa45 57 sonar.reset();
ckabuloglu 0:d9d5943dfa45 58 wait_us(10.0);
ckabuloglu 0:d9d5943dfa45 59 trigger = 0;
ckabuloglu 0:d9d5943dfa45 60
ckabuloglu 0:d9d5943dfa45 61 while (echo==0) {};
ckabuloglu 0:d9d5943dfa45 62 sonar.start();
ckabuloglu 0:d9d5943dfa45 63 while (echo==1) {};
ckabuloglu 0:d9d5943dfa45 64
ckabuloglu 0:d9d5943dfa45 65 sonar.stop();
ckabuloglu 0:d9d5943dfa45 66
ckabuloglu 0:d9d5943dfa45 67 distance = (sonar.read_us() - correction) / 58.0;
ckabuloglu 0:d9d5943dfa45 68 return distance;
ckabuloglu 0:d9d5943dfa45 69 }
ckabuloglu 0:d9d5943dfa45 70
ckabuloglu 0:d9d5943dfa45 71 int main() {
ckabuloglu 0:d9d5943dfa45 72 bool tooClose = false;
ckabuloglu 0:d9d5943dfa45 73 sonar.reset();
ckabuloglu 0:d9d5943dfa45 74 sonar.start();
ckabuloglu 0:d9d5943dfa45 75
ckabuloglu 0:d9d5943dfa45 76 while (echo==2) {};
ckabuloglu 0:d9d5943dfa45 77 sonar.stop();
ckabuloglu 0:d9d5943dfa45 78 int correction = sonar.read_us();
ckabuloglu 0:d9d5943dfa45 79 int distance = -1;
ckabuloglu 0:d9d5943dfa45 80
ckabuloglu 0:d9d5943dfa45 81 char bnum=0;
ckabuloglu 0:d9d5943dfa45 82 char bhit=0;
ckabuloglu 0:d9d5943dfa45 83
ckabuloglu 0:d9d5943dfa45 84 servo = 0.5;
ckabuloglu 0:d9d5943dfa45 85
ckabuloglu 0:d9d5943dfa45 86 while(1) {
ckabuloglu 0:d9d5943dfa45 87 if (blue.getc()=='!') {
ckabuloglu 0:d9d5943dfa45 88 if (blue.getc()=='B') { //button data packet
ckabuloglu 0:d9d5943dfa45 89 bnum = blue.getc(); //button number
ckabuloglu 0:d9d5943dfa45 90 bhit = blue.getc(); //1=hit, 0=release
ckabuloglu 0:d9d5943dfa45 91 if ((bnum>='1')&&(bnum<='8')) { //checksum OK?
ckabuloglu 0:d9d5943dfa45 92 chooseLed(2);
ckabuloglu 0:d9d5943dfa45 93 pc.printf("%d",0);
ckabuloglu 0:d9d5943dfa45 94 distance = getSonar(correction); // read sonar data
ckabuloglu 0:d9d5943dfa45 95 // wait(0.1); // wait echo signal to disperse
ckabuloglu 0:d9d5943dfa45 96
ckabuloglu 0:d9d5943dfa45 97 // check if the distance is too close, set the flag 'tooClose'
ckabuloglu 0:d9d5943dfa45 98 if (distance > 20) {
ckabuloglu 0:d9d5943dfa45 99 led3 = 0;
ckabuloglu 0:d9d5943dfa45 100 led4 = 1;
ckabuloglu 0:d9d5943dfa45 101 tooClose = false;
ckabuloglu 0:d9d5943dfa45 102 } else {
ckabuloglu 0:d9d5943dfa45 103 led3 = 1;
ckabuloglu 0:d9d5943dfa45 104 led4 = 0;
ckabuloglu 0:d9d5943dfa45 105 tooClose = true;
ckabuloglu 0:d9d5943dfa45 106 }
ckabuloglu 0:d9d5943dfa45 107
ckabuloglu 0:d9d5943dfa45 108 switch (bnum) {
ckabuloglu 0:d9d5943dfa45 109 case '1': //button 1 - clockwise servo
ckabuloglu 0:d9d5943dfa45 110 chooseLed(3);
ckabuloglu 0:d9d5943dfa45 111 pc.printf("%d",6);
ckabuloglu 0:d9d5943dfa45 112 if (bhit=='1') {
ckabuloglu 0:d9d5943dfa45 113 servo = 1.0;
ckabuloglu 0:d9d5943dfa45 114 } else {
ckabuloglu 0:d9d5943dfa45 115 servo = 0.5;
ckabuloglu 0:d9d5943dfa45 116 }
ckabuloglu 0:d9d5943dfa45 117 break;
ckabuloglu 0:d9d5943dfa45 118
ckabuloglu 0:d9d5943dfa45 119 case '2': //button 2 - counter clockwise servo
ckabuloglu 0:d9d5943dfa45 120 chooseLed(4);
ckabuloglu 0:d9d5943dfa45 121 pc.printf("%d",7);
ckabuloglu 0:d9d5943dfa45 122 if (bhit=='1') {
ckabuloglu 0:d9d5943dfa45 123 servo = 0;
ckabuloglu 0:d9d5943dfa45 124 } else {
ckabuloglu 0:d9d5943dfa45 125 servo = 0.5;
ckabuloglu 0:d9d5943dfa45 126 }
ckabuloglu 0:d9d5943dfa45 127 break;
ckabuloglu 0:d9d5943dfa45 128
ckabuloglu 0:d9d5943dfa45 129 case '5': //button 5 up arrow - move forward
ckabuloglu 0:d9d5943dfa45 130 chooseLed(1);
ckabuloglu 0:d9d5943dfa45 131 pc.printf("%d",1);
ckabuloglu 0:d9d5943dfa45 132 if (bhit=='1' && tooClose == 0) {
ckabuloglu 0:d9d5943dfa45 133 motor_left.speed(1);
ckabuloglu 0:d9d5943dfa45 134 motor_right.speed(1);
ckabuloglu 0:d9d5943dfa45 135 mySpeaker.PlayNote(440, 1.0, 0.2);
ckabuloglu 0:d9d5943dfa45 136 } else if (tooClose == 1) {
ckabuloglu 0:d9d5943dfa45 137 motor_left.speed(0);
ckabuloglu 0:d9d5943dfa45 138 motor_right.speed(0);
ckabuloglu 0:d9d5943dfa45 139 mySpeaker.PlayNote(880, 0.1, 1.0);
ckabuloglu 0:d9d5943dfa45 140 } else {
ckabuloglu 0:d9d5943dfa45 141 motor_left.speed(0);
ckabuloglu 0:d9d5943dfa45 142 motor_right.speed(0);
ckabuloglu 0:d9d5943dfa45 143 }
ckabuloglu 0:d9d5943dfa45 144 break;
ckabuloglu 0:d9d5943dfa45 145 case '6': //button 6 down arrow - move backward
ckabuloglu 0:d9d5943dfa45 146 chooseLed(2);
ckabuloglu 0:d9d5943dfa45 147 pc.printf("%d",2);
ckabuloglu 0:d9d5943dfa45 148 if (bhit=='1') {
ckabuloglu 0:d9d5943dfa45 149 motor_left.speed(-1);
ckabuloglu 0:d9d5943dfa45 150 motor_right.speed(-1);
ckabuloglu 0:d9d5943dfa45 151 mySpeaker.PlayNote(600, 0.5, 0.6);
ckabuloglu 0:d9d5943dfa45 152 } else {
ckabuloglu 0:d9d5943dfa45 153 motor_left.speed(0);
ckabuloglu 0:d9d5943dfa45 154 motor_right.speed(0);
ckabuloglu 0:d9d5943dfa45 155 }
ckabuloglu 0:d9d5943dfa45 156 break;
ckabuloglu 0:d9d5943dfa45 157 case '7': //button 7 left arrow - rotate left
ckabuloglu 0:d9d5943dfa45 158 chooseLed(3);
ckabuloglu 0:d9d5943dfa45 159 pc.printf("%d",3);
ckabuloglu 0:d9d5943dfa45 160 if (bhit=='1') {
ckabuloglu 0:d9d5943dfa45 161 motor_left.speed(-1);
ckabuloglu 0:d9d5943dfa45 162 motor_right.speed(1);
ckabuloglu 0:d9d5943dfa45 163 } else {
ckabuloglu 0:d9d5943dfa45 164 motor_left.speed(0);
ckabuloglu 0:d9d5943dfa45 165 motor_right.speed(0);
ckabuloglu 0:d9d5943dfa45 166 }
ckabuloglu 0:d9d5943dfa45 167 break;
ckabuloglu 0:d9d5943dfa45 168 case '8': //button 8 right arrow - rotate right
ckabuloglu 0:d9d5943dfa45 169 chooseLed(4);
ckabuloglu 0:d9d5943dfa45 170 pc.printf("%d",4);
ckabuloglu 0:d9d5943dfa45 171 if (bhit=='1') {
ckabuloglu 0:d9d5943dfa45 172 motor_left.speed(1);
ckabuloglu 0:d9d5943dfa45 173 motor_right.speed(-1);
ckabuloglu 0:d9d5943dfa45 174 } else {
ckabuloglu 0:d9d5943dfa45 175 motor_left.speed(0);
ckabuloglu 0:d9d5943dfa45 176 motor_right.speed(0);
ckabuloglu 0:d9d5943dfa45 177 }
ckabuloglu 0:d9d5943dfa45 178 break;
ckabuloglu 0:d9d5943dfa45 179 default:
ckabuloglu 0:d9d5943dfa45 180 break;
ckabuloglu 0:d9d5943dfa45 181 }
ckabuloglu 0:d9d5943dfa45 182 }
ckabuloglu 0:d9d5943dfa45 183 }
ckabuloglu 0:d9d5943dfa45 184 }
ckabuloglu 0:d9d5943dfa45 185 }
ckabuloglu 0:d9d5943dfa45 186 }