Sowmiya Sivanandham / Mbed 2 deprecated Final_Sonar

seeing i robot - with all the file systems and complete code

Dependencies:   mbed SRF05 Servo CMPS03

main.cpp@1:2bac7b6f3840, 2010-12-17 (annotated)

Committer:
sowmy87
Date:
Fri Dec 17 22:55:25 2010 +0000
Revision:
1:2bac7b6f3840
Parent:
0:ee786e500a3c 
Sensor 1 edited

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sowmy87 0:ee786e500a3c 1 #include "mbed.h"
sowmy87 0:ee786e500a3c 2 #include "Servo.h"
sowmy87 0:ee786e500a3c 3 #include "SRF05.h"
sowmy87 0:ee786e500a3c 4 #include "VS1002.h"
sowmy87 0:ee786e500a3c 5 #include "Serializer.h"
sowmy87 0:ee786e500a3c 6 #include "CMPS03.h"
sowmy87 0:ee786e500a3c 7 #include "math.h"
sowmy87 0:ee786e500a3c 8
sowmy87 0:ee786e500a3c 9 SRF05 srf1(p22, p23);
sowmy87 0:ee786e500a3c 10 SRF05 srf2(p24, p25);
sowmy87 0:ee786e500a3c 11 Serial pc(USBTX, USBRX);
sowmy87 0:ee786e500a3c 12 Servo myservo(p21);
sowmy87 1:2bac7b6f3840 13 VS1002 mp3(p5, p6, p7, p8, "sd",p11, p12, p13, p14, p15,p16, p17, p20); //p14 in BoB non-functional so replace with p18
sowmy87 0:ee786e500a3c 14 Serializer robot;
sowmy87 1:2bac7b6f3840 15 CMPS03 compass(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
sowmy87 0:ee786e500a3c 16
sowmy87 0:ee786e500a3c 17
sowmy87 0:ee786e500a3c 18 float motor_turn();
sowmy87 0:ee786e500a3c 19 void deviate_right(float angle);
sowmy87 0:ee786e500a3c 20 void deviate_left(float angle);
sowmy87 0:ee786e500a3c 21 int free_space_found = 0;
sowmy87 0:ee786e500a3c 22 int counter =1;
sowmy87 0:ee786e500a3c 23
sowmy87 0:ee786e500a3c 24 int main()
sowmy87 0:ee786e500a3c 25 {
sowmy87 0:ee786e500a3c 26
sowmy87 0:ee786e500a3c 27 float angle = 0;
sowmy87 0:ee786e500a3c 28 float distance1 = 200;
sowmy87 0:ee786e500a3c 29 float distance2 = 0;
sowmy87 0:ee786e500a3c 30 int obstacle = 0;
sowmy87 0:ee786e500a3c 31 int obstacle_count = 0;
sowmy87 0:ee786e500a3c 32 myservo.calibrate(.0009, 180);
sowmy87 0:ee786e500a3c 33
sowmy87 0:ee786e500a3c 34 #ifndef FS_ONLY
sowmy87 0:ee786e500a3c 35 mp3._RST = 1;
sowmy87 0:ee786e500a3c 36 mp3.cs_high(); //chip disabled
sowmy87 0:ee786e500a3c 37 mp3.sci_initialise();
sowmy87 0:ee786e500a3c 38 pc.printf( "\n\rInitializing SCI!");
sowmy87 0:ee786e500a3c 39 //initialise MBED
sowmy87 0:ee786e500a3c 40 mp3.sci_write(0x00,(SM_SDINEW+SM_STREAM+SM_DIFF));
sowmy87 0:ee786e500a3c 41 pc.printf( "\n\rWrite1!");
sowmy87 0:ee786e500a3c 42
sowmy87 0:ee786e500a3c 43 mp3.sci_write(0x03, 0x9800);
sowmy87 0:ee786e500a3c 44 pc.printf( "\n\rWrite2!");
sowmy87 0:ee786e500a3c 45
sowmy87 0:ee786e500a3c 46 mp3.sdi_initialise();
sowmy87 0:ee786e500a3c 47 pc.printf("\n\rInitialing SDI!");
sowmy87 0:ee786e500a3c 48
sowmy87 0:ee786e500a3c 49 #endif
sowmy87 0:ee786e500a3c 50 while(1)
sowmy87 0:ee786e500a3c 51 {
sowmy87 0:ee786e500a3c 52 pc.printf("\n\rNew code!!!");
sowmy87 0:ee786e500a3c 53 myservo = 0.5;
sowmy87 0:ee786e500a3c 54 wait(1);
sowmy87 1:2bac7b6f3840 55 distance1 = srf1.read();
sowmy87 0:ee786e500a3c 56 // distance2 = 200;
sowmy87 1:2bac7b6f3840 57 //distance1 = 200;
sowmy87 0:ee786e500a3c 58 distance2 = srf2.read();
sowmy87 0:ee786e500a3c 59 pc.printf("\n\rDistance1:%f", distance1);
sowmy87 0:ee786e500a3c 60 pc.printf("\n\rDistance2:%f", distance2);
sowmy87 0:ee786e500a3c 61 if(distance1 > 100 && distance2 > 120 )
sowmy87 0:ee786e500a3c 62 {
sowmy87 0:ee786e500a3c 63 pc.printf("\n\r No obstacle! Go straight");
sowmy87 0:ee786e500a3c 64 obstacle = 0;
sowmy87 0:ee786e500a3c 65 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 66
sowmy87 0:ee786e500a3c 67 }
sowmy87 0:ee786e500a3c 68 else
sowmy87 0:ee786e500a3c 69 {
sowmy87 0:ee786e500a3c 70 obstacle = 1;
sowmy87 0:ee786e500a3c 71 obstacle_count++;
sowmy87 0:ee786e500a3c 72 // robot.Stop();
sowmy87 0:ee786e500a3c 73 }
sowmy87 0:ee786e500a3c 74
sowmy87 0:ee786e500a3c 75 if(obstacle_count == 1)
sowmy87 0:ee786e500a3c 76 {
sowmy87 0:ee786e500a3c 77 obstacle_count = 0;
sowmy87 0:ee786e500a3c 78 robot.Stop();
sowmy87 0:ee786e500a3c 79 mp3.play_song(11);
sowmy87 0:ee786e500a3c 80 angle = motor_turn();
sowmy87 0:ee786e500a3c 81 if( angle == -1000 )
sowmy87 0:ee786e500a3c 82 {
sowmy87 0:ee786e500a3c 83 pc.printf("\n\rStuck!");
sowmy87 0:ee786e500a3c 84 // robot.SetSpeed(-70);
sowmy87 0:ee786e500a3c 85 //go back
sowmy87 0:ee786e500a3c 86
sowmy87 0:ee786e500a3c 87 }
sowmy87 0:ee786e500a3c 88 else if (angle > 0)
sowmy87 0:ee786e500a3c 89 {
sowmy87 0:ee786e500a3c 90 pc.printf("\n\rCalling deviate_right(%f)", angle);
sowmy87 0:ee786e500a3c 91 deviate_right(angle);
sowmy87 0:ee786e500a3c 92 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 93 // robot.PivetRight(angle);
sowmy87 0:ee786e500a3c 94 }
sowmy87 0:ee786e500a3c 95 else if(angle < 0 )
sowmy87 0:ee786e500a3c 96 {
sowmy87 0:ee786e500a3c 97 pc.printf("\n\rCalling deviate_left(%f)", angle);
sowmy87 0:ee786e500a3c 98 deviate_left(angle);
sowmy87 0:ee786e500a3c 99 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 100 //robot.PivetLeft(abs (angle));
sowmy87 0:ee786e500a3c 101 }
sowmy87 0:ee786e500a3c 102 else if (angle == 0)
sowmy87 0:ee786e500a3c 103 {
sowmy87 0:ee786e500a3c 104 pc.printf("\n\rContine going straight!");
sowmy87 0:ee786e500a3c 105 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 106 }
sowmy87 0:ee786e500a3c 107 }
sowmy87 0:ee786e500a3c 108
sowmy87 0:ee786e500a3c 109 }
sowmy87 0:ee786e500a3c 110 }
sowmy87 0:ee786e500a3c 111
sowmy87 0:ee786e500a3c 112
sowmy87 0:ee786e500a3c 113 float motor_turn()
sowmy87 0:ee786e500a3c 114 {
sowmy87 0:ee786e500a3c 115 free_space_found = 0;
sowmy87 0:ee786e500a3c 116 counter = 1;
sowmy87 0:ee786e500a3c 117 float distance1 = 0;
sowmy87 0:ee786e500a3c 118 float distance2 = 0;
sowmy87 0:ee786e500a3c 119 for(float i = 0.5; i <= 1; i=i+0.16)
sowmy87 0:ee786e500a3c 120 {
sowmy87 0:ee786e500a3c 121 myservo = i;
sowmy87 0:ee786e500a3c 122 wait(1);
sowmy87 1:2bac7b6f3840 123 distance1 = srf1.read();
sowmy87 0:ee786e500a3c 124 distance2 = srf2.read();
sowmy87 0:ee786e500a3c 125 pc.printf("\n\rDistance1:%f", distance1);
sowmy87 0:ee786e500a3c 126 pc.printf("\n\rDistance2:%f", distance2);
sowmy87 0:ee786e500a3c 127 pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
sowmy87 0:ee786e500a3c 128 pc.printf("\n\ri:%f", i);
sowmy87 0:ee786e500a3c 129
sowmy87 0:ee786e500a3c 130 if( distance1 > 100 && distance2 > 120)
sowmy87 0:ee786e500a3c 131 {
sowmy87 0:ee786e500a3c 132 pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
sowmy87 0:ee786e500a3c 133 free_space_found = 1;
sowmy87 0:ee786e500a3c 134 mp3.play_song(counter);
sowmy87 0:ee786e500a3c 135 return (180*(i-0.5));
sowmy87 0:ee786e500a3c 136
sowmy87 0:ee786e500a3c 137 }
sowmy87 0:ee786e500a3c 138 counter++;
sowmy87 0:ee786e500a3c 139 }
sowmy87 0:ee786e500a3c 140 if( free_space_found == 0 )
sowmy87 0:ee786e500a3c 141 {
sowmy87 0:ee786e500a3c 142 pc.printf("\n\rNo free space on right side!!");
sowmy87 0:ee786e500a3c 143 for(float i = 0.5; i >= 0 ; i=i-0.16)
sowmy87 0:ee786e500a3c 144 {
sowmy87 0:ee786e500a3c 145 myservo = i;
sowmy87 0:ee786e500a3c 146 wait(1);
sowmy87 1:2bac7b6f3840 147 distance1 = srf1.read();
sowmy87 0:ee786e500a3c 148 distance2 = srf2.read();
sowmy87 0:ee786e500a3c 149 pc.printf("\n\rDistance1:%f", distance1);
sowmy87 0:ee786e500a3c 150 pc.printf("\n\rDistance2:%f", distance2);
sowmy87 0:ee786e500a3c 151 pc.printf("\n\ri:%f", i);
sowmy87 0:ee786e500a3c 152 pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
sowmy87 0:ee786e500a3c 153 if( distance1 > 100 && distance2 > 120 )
sowmy87 0:ee786e500a3c 154 {
sowmy87 0:ee786e500a3c 155 pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
sowmy87 0:ee786e500a3c 156 free_space_found = 1;
sowmy87 0:ee786e500a3c 157 mp3.play_song(counter);
sowmy87 0:ee786e500a3c 158 return (180*(i-0.5));
sowmy87 0:ee786e500a3c 159
sowmy87 0:ee786e500a3c 160 }
sowmy87 0:ee786e500a3c 161 counter++;
sowmy87 0:ee786e500a3c 162 }
sowmy87 0:ee786e500a3c 163 }
sowmy87 0:ee786e500a3c 164
sowmy87 0:ee786e500a3c 165 if( free_space_found == 0)
sowmy87 0:ee786e500a3c 166 {
sowmy87 0:ee786e500a3c 167 pc.printf("\n\rNo free space found! Go back!");
sowmy87 0:ee786e500a3c 168 mp3.play_song(10);
sowmy87 0:ee786e500a3c 169 return(-1000);
sowmy87 0:ee786e500a3c 170 }
sowmy87 0:ee786e500a3c 171 return 0.0;
sowmy87 0:ee786e500a3c 172 }
sowmy87 0:ee786e500a3c 173
sowmy87 0:ee786e500a3c 174
sowmy87 0:ee786e500a3c 175 void deviate_right(float angle)
sowmy87 0:ee786e500a3c 176 {
sowmy87 0:ee786e500a3c 177 float distance1 = 0;
sowmy87 0:ee786e500a3c 178 float distance2 = 0;
sowmy87 0:ee786e500a3c 179 float distance3 = 0;
sowmy87 0:ee786e500a3c 180 float distance4 =0;
sowmy87 0:ee786e500a3c 181 float i = 0.5;
sowmy87 0:ee786e500a3c 182 float deviation = 0;//Check for float with pavel
sowmy87 0:ee786e500a3c 183 int count_deviation = 0;
sowmy87 0:ee786e500a3c 184 pc.printf("\n\rInside deviate_right()");
sowmy87 0:ee786e500a3c 185 robot.PivetRight(angle);
sowmy87 0:ee786e500a3c 186 wait(2);
sowmy87 0:ee786e500a3c 187 pc.printf("\n\rPivetRight(%f)",angle);
sowmy87 0:ee786e500a3c 188 robot.ClearCount();
sowmy87 0:ee786e500a3c 189 pc.printf("\n\rClear count!");
sowmy87 0:ee786e500a3c 190 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 191 pc.printf("\n\rSetSpeed(70)");
sowmy87 0:ee786e500a3c 192 while(1)
sowmy87 0:ee786e500a3c 193 {
sowmy87 0:ee786e500a3c 194 pc.printf("\n\rInside while!!!");
sowmy87 0:ee786e500a3c 195 count_deviation = 0;
sowmy87 0:ee786e500a3c 196 myservo = 0.5;
sowmy87 0:ee786e500a3c 197 wait(1);
sowmy87 1:2bac7b6f3840 198 distance1 = srf1.read();
sowmy87 0:ee786e500a3c 199 distance2 = srf2.read();
sowmy87 0:ee786e500a3c 200 i = (-1*angle/180 + 0.5);
sowmy87 0:ee786e500a3c 201 myservo = i;
sowmy87 0:ee786e500a3c 202 wait(1);
sowmy87 1:2bac7b6f3840 203 distance3 = srf1.read();
sowmy87 1:2bac7b6f3840 204 distance4 = srf2.read();
sowmy87 0:ee786e500a3c 205
sowmy87 0:ee786e500a3c 206 if(distance3 > 100 && distance4 > 120)
sowmy87 0:ee786e500a3c 207 {
sowmy87 0:ee786e500a3c 208 pc.printf("\n\rFree space at 2*i");
sowmy87 0:ee786e500a3c 209 deviation = robot.GetDistance();
sowmy87 0:ee786e500a3c 210 count_deviation = robot.GetCount();
sowmy87 0:ee786e500a3c 211 pc.printf("\n\rDeviation: %f, Count_deviation:%d", deviation, count_deviation);
sowmy87 0:ee786e500a3c 212 robot.Stop();
sowmy87 0:ee786e500a3c 213 mp3.play_song(11);
sowmy87 0:ee786e500a3c 214 pc.printf("\n\rStop!");
sowmy87 0:ee786e500a3c 215 robot.PivetLeft(angle);
sowmy87 0:ee786e500a3c 216 mp3.play_song(counter+4);
sowmy87 0:ee786e500a3c 217 wait(2);
sowmy87 0:ee786e500a3c 218 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 219 pc.printf("\n\rPivetleft(2*%f)", angle);
sowmy87 0:ee786e500a3c 220 // robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
sowmy87 0:ee786e500a3c 221 // pc.printf("\n\rDiGo(%d,70)", count_deviation);
sowmy87 0:ee786e500a3c 222 /* while(count_deviation != 0 )
sowmy87 0:ee786e500a3c 223 {
sowmy87 0:ee786e500a3c 224 pc.printf("\n\rcount_deviation:%d", count_deviation);
sowmy87 0:ee786e500a3c 225 count_deviation --;
sowmy87 0:ee786e500a3c 226 wait(0.01);
sowmy87 0:ee786e500a3c 227 }*/
sowmy87 0:ee786e500a3c 228 // wait(5);
sowmy87 0:ee786e500a3c 229 // robot.Stop();//??
sowmy87 0:ee786e500a3c 230 // pc.printf("\n\rStop!");
sowmy87 0:ee786e500a3c 231 // robot.PivetRight(angle);
sowmy87 0:ee786e500a3c 232 // pc.printf("\n\rPivetRight(%f)", angle);
sowmy87 0:ee786e500a3c 233 return;
sowmy87 0:ee786e500a3c 234 }
sowmy87 0:ee786e500a3c 235 if( distance1 < 100 || distance2 < 120 ) //changed!
sowmy87 0:ee786e500a3c 236 {
sowmy87 0:ee786e500a3c 237 pc.printf("\n\rStuck!");
sowmy87 0:ee786e500a3c 238 robot.Stop();
sowmy87 0:ee786e500a3c 239 mp3.play_song(11);
sowmy87 0:ee786e500a3c 240 //goback
sowmy87 0:ee786e500a3c 241 }
sowmy87 0:ee786e500a3c 242
sowmy87 0:ee786e500a3c 243 }
sowmy87 0:ee786e500a3c 244 }
sowmy87 0:ee786e500a3c 245
sowmy87 0:ee786e500a3c 246
sowmy87 0:ee786e500a3c 247 void deviate_left(float angle)
sowmy87 0:ee786e500a3c 248 {
sowmy87 0:ee786e500a3c 249 float distance1 = 0;
sowmy87 0:ee786e500a3c 250 float distance2 = 0;
sowmy87 0:ee786e500a3c 251 float distance3 = 0;
sowmy87 0:ee786e500a3c 252 float distance4 = 0;
sowmy87 0:ee786e500a3c 253 float i = 0.5;
sowmy87 0:ee786e500a3c 254 float deviation = 0;//Check for float with pavel
sowmy87 0:ee786e500a3c 255 int count_deviation = 0;
sowmy87 0:ee786e500a3c 256 robot.PivetLeft(-angle);
sowmy87 0:ee786e500a3c 257 wait(2);
sowmy87 0:ee786e500a3c 258 pc.printf("\n\rInside deviate_left!!");
sowmy87 0:ee786e500a3c 259 pc.printf("\n\rPivetLeft(%f)", -angle);
sowmy87 0:ee786e500a3c 260 robot.ClearCount();
sowmy87 0:ee786e500a3c 261 pc.printf("\n\rClearCount");
sowmy87 0:ee786e500a3c 262 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 263 pc.printf("\n\rSetSpeed(70)");
sowmy87 0:ee786e500a3c 264 while(1)
sowmy87 0:ee786e500a3c 265 {
sowmy87 0:ee786e500a3c 266 count_deviation = 0;
sowmy87 0:ee786e500a3c 267 myservo = 0.5;
sowmy87 0:ee786e500a3c 268 wait(1);
sowmy87 1:2bac7b6f3840 269 distance1 = srf1.read();
sowmy87 0:ee786e500a3c 270 distance2 = srf2.read();
sowmy87 0:ee786e500a3c 271 i = (-1*angle/180 + 0.5);
sowmy87 0:ee786e500a3c 272 myservo = i;
sowmy87 0:ee786e500a3c 273 wait(1);
sowmy87 1:2bac7b6f3840 274 distance3 = srf1.read();
sowmy87 0:ee786e500a3c 275 distance4 = srf2.read();
sowmy87 0:ee786e500a3c 276 if(distance3 > 100 && distance4 > 120)
sowmy87 0:ee786e500a3c 277 {
sowmy87 0:ee786e500a3c 278 pc.printf("\n\r Free space at 2*i!");
sowmy87 0:ee786e500a3c 279 deviation = robot.GetDistance();
sowmy87 0:ee786e500a3c 280 count_deviation = robot.GetCount();
sowmy87 0:ee786e500a3c 281 pc.printf("\n\rDeviation:%f, count_deviation:%d", deviation, count_deviation);
sowmy87 0:ee786e500a3c 282 robot.Stop();
sowmy87 0:ee786e500a3c 283 mp3.play_song(11);
sowmy87 0:ee786e500a3c 284 pc.printf("\n\rStop!");
sowmy87 0:ee786e500a3c 285 robot.PivetRight(-1*angle);
sowmy87 0:ee786e500a3c 286 mp3.play_song(counter-4);
sowmy87 0:ee786e500a3c 287 wait(2);
sowmy87 0:ee786e500a3c 288 pc.printf("\n\rPivetRight(2*%f)", -angle);
sowmy87 0:ee786e500a3c 289 robot.SetSpeed(70);
sowmy87 0:ee786e500a3c 290 // robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
sowmy87 0:ee786e500a3c 291 // pc.printf("\n\rDigo(%d,70)", count_deviation);
sowmy87 0:ee786e500a3c 292 /* while(count_deviation != 0 )
sowmy87 0:ee786e500a3c 293 {
sowmy87 0:ee786e500a3c 294 pc.printf("\n\rcount_deviation:%d",count_deviation);
sowmy87 0:ee786e500a3c 295 count_deviation --;
sowmy87 0:ee786e500a3c 296 wait(0.01);
sowmy87 0:ee786e500a3c 297 }*/
sowmy87 0:ee786e500a3c 298 // wait(5);
sowmy87 0:ee786e500a3c 299 // robot.Stop();//??
sowmy87 0:ee786e500a3c 300 //// pc.printf("\n\rStop!");
sowmy87 0:ee786e500a3c 301 // robot.PivetLeft(-angle);
sowmy87 0:ee786e500a3c 302 // wait(5);
sowmy87 0:ee786e500a3c 303 // pc.printf("\n\rPivetLeft(%f)", -angle);
sowmy87 0:ee786e500a3c 304 return;
sowmy87 0:ee786e500a3c 305 }
sowmy87 0:ee786e500a3c 306 if( distance1 < 100 || distance2 < 120 ) //changed
sowmy87 0:ee786e500a3c 307 {
sowmy87 0:ee786e500a3c 308 pc.printf("\n\rStuck!");
sowmy87 0:ee786e500a3c 309 robot.Stop();
sowmy87 0:ee786e500a3c 310 mp3.play_song(11);
sowmy87 0:ee786e500a3c 311 float ang = 0;
sowmy87 0:ee786e500a3c 312 // ang = motor_turn();
sowmy87 0:ee786e500a3c 313
sowmy87 0:ee786e500a3c 314
sowmy87 0:ee786e500a3c 315 //goback
sowmy87 0:ee786e500a3c 316 }
sowmy87 0:ee786e500a3c 317 }
sowmy87 0:ee786e500a3c 318
sowmy87 0:ee786e500a3c 319 }