chong wang / Mbed 2 deprecated AutoMapGuidedRobot

A final design project for embedded system class

Dependencies:   HMC6352 mbed

main.cpp@0:c2c6f1a295f4, 2013-04-26 (annotated)

Committer:
cwang365
Date:
Fri Apr 26 17:42:39 2013 +0000
Revision:
0:c2c6f1a295f4
Final version of autonomous map guided robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cwang365 0:c2c6f1a295f4 1
cwang365 0:c2c6f1a295f4 2 //#include "HMC6352.h" //compass
cwang365 0:c2c6f1a295f4 3 #include "mbed.h"
cwang365 0:c2c6f1a295f4 4 //HMC6352 compass(p28, p27);
cwang365 0:c2c6f1a295f4 5 Serial pc(USBTX, USBRX);
cwang365 0:c2c6f1a295f4 6 AnalogIn Rdist(p19); // right sonar
cwang365 0:c2c6f1a295f4 7 AnalogIn Ldist(p20);// left sonar
cwang365 0:c2c6f1a295f4 8 AnalogIn Fdist(p18);// front sonar
cwang365 0:c2c6f1a295f4 9 AnalogIn range(p17); // right range finder
cwang365 0:c2c6f1a295f4 10 Serial device(p9, p10); // tx, rx
cwang365 0:c2c6f1a295f4 11 DigitalOut leftlight(LED1);
cwang365 0:c2c6f1a295f4 12 DigitalOut rightlight(LED3);
cwang365 0:c2c6f1a295f4 13 DigitalOut straightlight(LED2);
cwang365 0:c2c6f1a295f4 14
cwang365 0:c2c6f1a295f4 15 // Definitions of iRobot Create OpenInterface Command Numbers
cwang365 0:c2c6f1a295f4 16 // See the Create OpenInterface manual for a complete list
cwang365 0:c2c6f1a295f4 17
cwang365 0:c2c6f1a295f4 18
cwang365 0:c2c6f1a295f4 19 // Create Command // Arguments
cwang365 0:c2c6f1a295f4 20 const char Start = 128;
cwang365 0:c2c6f1a295f4 21 const char SafeMode = 131;
cwang365 0:c2c6f1a295f4 22 const char FullMode = 132;
cwang365 0:c2c6f1a295f4 23 const char Drive = 137; // 4: [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
cwang365 0:c2c6f1a295f4 24 const char DriveDirect = 145; // 4: [Right Hi] [Right Low] [Left Hi] [Left Low]
cwang365 0:c2c6f1a295f4 25 const char Demo = 136; // 2: Run Demo x
cwang365 0:c2c6f1a295f4 26 const char Sensors = 142; // 1: Sensor Packet ID
cwang365 0:c2c6f1a295f4 27 const char CoverandDock = 143; // 1: Return to Charger
cwang365 0:c2c6f1a295f4 28 const char SensorStream = 148; // x+1: [# of packets requested] IDs of requested packets to stream
cwang365 0:c2c6f1a295f4 29 const char QueryList = 149; // x+1: [# of packets requested] IDs of requested packets to stream
cwang365 0:c2c6f1a295f4 30 const char StreamPause = 150; // 1: 0 = stop stream, 1 = start stream
cwang365 0:c2c6f1a295f4 31 const char PlaySong = 141;
cwang365 0:c2c6f1a295f4 32 const char Song = 140;
cwang365 0:c2c6f1a295f4 33 /* iRobot Create Sensor IDs */
cwang365 0:c2c6f1a295f4 34 const char BumpsandDrops = 7;
cwang365 0:c2c6f1a295f4 35 const char Distance = 19;
cwang365 0:c2c6f1a295f4 36 const char Angle = 20;
cwang365 0:c2c6f1a295f4 37
cwang365 0:c2c6f1a295f4 38 static int speed_left = 200;
cwang365 0:c2c6f1a295f4 39 static int speed_right = 200;
cwang365 0:c2c6f1a295f4 40 static int state=0;
cwang365 0:c2c6f1a295f4 41 //static int arr[10];//0=forwatd, 1=right,2=left,3=finish;
cwang365 0:c2c6f1a295f4 42 void start();
cwang365 0:c2c6f1a295f4 43 void forward();
cwang365 0:c2c6f1a295f4 44 void reverse();
cwang365 0:c2c6f1a295f4 45 void left();
cwang365 0:c2c6f1a295f4 46 void right();
cwang365 0:c2c6f1a295f4 47 void stop();
cwang365 0:c2c6f1a295f4 48 void playsong();
cwang365 0:c2c6f1a295f4 49 void charger();
cwang365 0:c2c6f1a295f4 50 float curveR=0.02;
cwang365 0:c2c6f1a295f4 51 float curveL=0.02;
cwang365 0:c2c6f1a295f4 52 float Hsouth=98;
cwang365 0:c2c6f1a295f4 53 float Hnorth=68;
cwang365 0:c2c6f1a295f4 54 float Hwest=80;
cwang365 0:c2c6f1a295f4 55 float Heast=77;
cwang365 0:c2c6f1a295f4 56 // Demo to move around using basic commands
cwang365 0:c2c6f1a295f4 57 int main()
cwang365 0:c2c6f1a295f4 58 {
cwang365 0:c2c6f1a295f4 59 int arr[5]= {0,1,0,1,40};//0=forwatd, 1=right,2=left,3=finish;
cwang365 0:c2c6f1a295f4 60 float time=0;
cwang365 0:c2c6f1a295f4 61 // wait for Create to power up to accept serial commands
cwang365 0:c2c6f1a295f4 62 wait(3);
cwang365 0:c2c6f1a295f4 63 //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
cwang365 0:c2c6f1a295f4 64 // set baud rate for Create factory default
cwang365 0:c2c6f1a295f4 65 device.baud(57600);
cwang365 0:c2c6f1a295f4 66 // Start command mode and select sensor data to send back
cwang365 0:c2c6f1a295f4 67 start();
cwang365 0:c2c6f1a295f4 68 wait(.5);
cwang365 0:c2c6f1a295f4 69 // Move around with motor commands
cwang365 0:c2c6f1a295f4 70 while(1) {
cwang365 0:c2c6f1a295f4 71
cwang365 0:c2c6f1a295f4 72 if((Rdist>0.49 || Ldist>0.49)&&(arr[state]<3)) {
cwang365 0:c2c6f1a295f4 73 leftlight=1;
cwang365 0:c2c6f1a295f4 74 rightlight=1;
cwang365 0:c2c6f1a295f4 75 straightlight=1;
cwang365 0:c2c6f1a295f4 76 if(float(Fdist)<0.12 ) {
cwang365 0:c2c6f1a295f4 77 stop();
cwang365 0:c2c6f1a295f4 78 wait(1);
cwang365 0:c2c6f1a295f4 79 if(float(Fdist)<0.13 ) {
cwang365 0:c2c6f1a295f4 80 state++;
cwang365 0:c2c6f1a295f4 81 if(arr[state]==1) { //right turn
cwang365 0:c2c6f1a295f4 82 speed_right=200;
cwang365 0:c2c6f1a295f4 83 speed_left=200;
cwang365 0:c2c6f1a295f4 84 right();
cwang365 0:c2c6f1a295f4 85 wait(1.0);
cwang365 0:c2c6f1a295f4 86 } else if(arr[state]==2) {
cwang365 0:c2c6f1a295f4 87 speed_right=200;
cwang365 0:c2c6f1a295f4 88 speed_left=200;
cwang365 0:c2c6f1a295f4 89 left();
cwang365 0:c2c6f1a295f4 90 wait(1.0);
cwang365 0:c2c6f1a295f4 91 }//left
cwang365 0:c2c6f1a295f4 92 stop();
cwang365 0:c2c6f1a295f4 93 wait(0.2);
cwang365 0:c2c6f1a295f4 94 forward();
cwang365 0:c2c6f1a295f4 95 wait(6);
cwang365 0:c2c6f1a295f4 96 state++;
cwang365 0:c2c6f1a295f4 97 }
cwang365 0:c2c6f1a295f4 98 }
cwang365 0:c2c6f1a295f4 99
cwang365 0:c2c6f1a295f4 100 // else {
cwang365 0:c2c6f1a295f4 101 // speed_right=200;
cwang365 0:c2c6f1a295f4 102 // speed_left=200;
cwang365 0:c2c6f1a295f4 103 //}
cwang365 0:c2c6f1a295f4 104 } else if((float(Ldist)-float(Rdist))>curveL ) {
cwang365 0:c2c6f1a295f4 105 speed_right++;
cwang365 0:c2c6f1a295f4 106 leftlight=1;
cwang365 0:c2c6f1a295f4 107 rightlight=0;
cwang365 0:c2c6f1a295f4 108 straightlight=0;
cwang365 0:c2c6f1a295f4 109 if(speed_right>206) {
cwang365 0:c2c6f1a295f4 110 speed_right=206;
cwang365 0:c2c6f1a295f4 111 }
cwang365 0:c2c6f1a295f4 112 } else if ( (float(Rdist)-float(Ldist))>curveR ) {
cwang365 0:c2c6f1a295f4 113 speed_left++;
cwang365 0:c2c6f1a295f4 114 leftlight=0;
cwang365 0:c2c6f1a295f4 115 rightlight=1;
cwang365 0:c2c6f1a295f4 116 straightlight=0;
cwang365 0:c2c6f1a295f4 117 if(speed_left>206) {
cwang365 0:c2c6f1a295f4 118 speed_left=206;
cwang365 0:c2c6f1a295f4 119 }
cwang365 0:c2c6f1a295f4 120 }
cwang365 0:c2c6f1a295f4 121
cwang365 0:c2c6f1a295f4 122 else if (((float(Ldist)-float(Rdist))<curveL) && ( (float(Rdist)-float(Ldist))<curveR )) {
cwang365 0:c2c6f1a295f4 123 speed_left=200;
cwang365 0:c2c6f1a295f4 124 speed_right=200;
cwang365 0:c2c6f1a295f4 125 leftlight=0;
cwang365 0:c2c6f1a295f4 126 rightlight=0;
cwang365 0:c2c6f1a295f4 127 straightlight=1;
cwang365 0:c2c6f1a295f4 128 }
cwang365 0:c2c6f1a295f4 129 if(arr[state]>2) {
cwang365 0:c2c6f1a295f4 130 time=time+0.1;
cwang365 0:c2c6f1a295f4 131 if(time>arr[state]) {
cwang365 0:c2c6f1a295f4 132 break;//while
cwang365 0:c2c6f1a295f4 133 }
cwang365 0:c2c6f1a295f4 134 }
cwang365 0:c2c6f1a295f4 135
cwang365 0:c2c6f1a295f4 136 //pc.printf("Left: %f ", float(Ldist));
cwang365 0:c2c6f1a295f4 137 pc.printf("Right: %f ", float(Rdist));
cwang365 0:c2c6f1a295f4 138 pc.printf("range: %f ", float(range));
cwang365 0:c2c6f1a295f4 139 //pc.printf("front: %f ", float(Fdist));
cwang365 0:c2c6f1a295f4 140 //pc.printf("Diff: %f\n", (float(Rdist)-float(Ldist)));
cwang365 0:c2c6f1a295f4 141 forward();
cwang365 0:c2c6f1a295f4 142 wait(0.1);
cwang365 0:c2c6f1a295f4 143 }
cwang365 0:c2c6f1a295f4 144 // Play a song
cwang365 0:c2c6f1a295f4 145 stop();
cwang365 0:c2c6f1a295f4 146 playsong();
cwang365 0:c2c6f1a295f4 147 wait(10);
cwang365 0:c2c6f1a295f4 148 // Search for battery charger IR beacon
cwang365 0:c2c6f1a295f4 149 // charger();
cwang365 0:c2c6f1a295f4 150 }
cwang365 0:c2c6f1a295f4 151
cwang365 0:c2c6f1a295f4 152
cwang365 0:c2c6f1a295f4 153 // Start - send start and safe mode, start streaming sensor data
cwang365 0:c2c6f1a295f4 154 void start()
cwang365 0:c2c6f1a295f4 155 {
cwang365 0:c2c6f1a295f4 156 // device.printf("%c%c", Start, SafeMode);
cwang365 0:c2c6f1a295f4 157 device.putc(Start);
cwang365 0:c2c6f1a295f4 158 device.putc(SafeMode);
cwang365 0:c2c6f1a295f4 159 wait(.5);
cwang365 0:c2c6f1a295f4 160 // device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
cwang365 0:c2c6f1a295f4 161 device.putc(SensorStream);
cwang365 0:c2c6f1a295f4 162 device.putc(1);
cwang365 0:c2c6f1a295f4 163 device.putc(BumpsandDrops);
cwang365 0:c2c6f1a295f4 164 wait(.5);
cwang365 0:c2c6f1a295f4 165 }
cwang365 0:c2c6f1a295f4 166 // Stop - turn off drive motors
cwang365 0:c2c6f1a295f4 167 void stop()
cwang365 0:c2c6f1a295f4 168 {
cwang365 0:c2c6f1a295f4 169 device.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0));
cwang365 0:c2c6f1a295f4 170 }
cwang365 0:c2c6f1a295f4 171 // Forward - turn on drive motors
cwang365 0:c2c6f1a295f4 172 void forward()
cwang365 0:c2c6f1a295f4 173 {
cwang365 0:c2c6f1a295f4 174 device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
cwang365 0:c2c6f1a295f4 175 char((speed_left>>8)&0xFF), char(speed_left&0xFF));
cwang365 0:c2c6f1a295f4 176
cwang365 0:c2c6f1a295f4 177 }
cwang365 0:c2c6f1a295f4 178 // Reverse - reverse drive motors
cwang365 0:c2c6f1a295f4 179 void reverse()
cwang365 0:c2c6f1a295f4 180 {
cwang365 0:c2c6f1a295f4 181 device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
cwang365 0:c2c6f1a295f4 182 char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
cwang365 0:c2c6f1a295f4 183
cwang365 0:c2c6f1a295f4 184 }
cwang365 0:c2c6f1a295f4 185 // Left - drive motors set to rotate to left
cwang365 0:c2c6f1a295f4 186 void left()
cwang365 0:c2c6f1a295f4 187 {
cwang365 0:c2c6f1a295f4 188 device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
cwang365 0:c2c6f1a295f4 189 char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
cwang365 0:c2c6f1a295f4 190 }
cwang365 0:c2c6f1a295f4 191 // Right - drive motors set to rotate to right
cwang365 0:c2c6f1a295f4 192 void right()
cwang365 0:c2c6f1a295f4 193 {
cwang365 0:c2c6f1a295f4 194 device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
cwang365 0:c2c6f1a295f4 195 char((speed_left>>8)&0xFF), char(speed_left&0xFF));
cwang365 0:c2c6f1a295f4 196
cwang365 0:c2c6f1a295f4 197 }
cwang365 0:c2c6f1a295f4 198 // Charger - search and return to charger using IR beacons (if found)
cwang365 0:c2c6f1a295f4 199 void charger()
cwang365 0:c2c6f1a295f4 200 {
cwang365 0:c2c6f1a295f4 201 device.printf("%c%c", Demo, char(1));
cwang365 0:c2c6f1a295f4 202 }
cwang365 0:c2c6f1a295f4 203 // Play Song - define and play a song
cwang365 0:c2c6f1a295f4 204 void playsong() // Send out notes & duration to define song and then play song
cwang365 0:c2c6f1a295f4 205 {
cwang365 0:c2c6f1a295f4 206
cwang365 0:c2c6f1a295f4 207 device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
cwang365 0:c2c6f1a295f4 208 Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
cwang365 0:c2c6f1a295f4 209 char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
cwang365 0:c2c6f1a295f4 210 char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
cwang365 0:c2c6f1a295f4 211 char(24), char(86), char(12), char(87), char(48));
cwang365 0:c2c6f1a295f4 212
cwang365 0:c2c6f1a295f4 213 wait(.2);
cwang365 0:c2c6f1a295f4 214 device.printf("%c%c", PlaySong, char(0));
cwang365 0:c2c6f1a295f4 215 }