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@0:ee786e500a3c, 2010-12-17 (annotated)

Committer:
sowmy87
Date:
Fri Dec 17 18:27:33 2010 +0000
Revision:
0:ee786e500a3c
Child:
1:2bac7b6f3840
Completed

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