chong wang
/
AutoMapGuidedRobot
A final design project for embedded system class
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?
User | Revision | Line number | New 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 | } |